Help with initial approximation of parameters [X Y Z] for intersection by linear method
Show older comments
See BOLDED below (lines 68-70). I'm stumped on those three lines in this "fill in the code" assignment. Thank you for any gudiance!
clc;
clear all;
f0_mm=3.61; %mm
f0_px=3.61/6.16*4000; %pixel
data=readmatrix('intersection_pt.txt');
focal=f0_px; % update
W=4000;
H=3000;
pt_id=data(:,1);
x1=data(:,2)-W/2;
y1=H/2-data(:,3);
x2=data(:,4)-W/2;
y2=H/2-data(:,5);
[m,n]=size(data);
EO1= %%% You can find the EO information from the Lab slide
EO2= %%%
XL1=EO1(1,1);
YL1=EO1(2,1);
ZL1=EO1(3,1);
o1=EO1(4,1);
p1=EO1(5,1);
k1=EO1(6,1);
XL2=EO2(1,1);
YL2=EO2(2,1);
ZL2=EO2(3,1);
o2=EO2(4,1);
p2=EO2(5,1);
k2=EO2(6,1);
M1=[cos(p1)*cos(k1) cos(o1)*sin(k1)+sin(o1)*sin(p1)*cos(k1) sin(o1)*sin(k1)-cos(o1)*sin(p1)*cos(k1);
-cos(p1)*sin(k1) cos(o1)*cos(k1)-sin(o1)*sin(p1)*sin(k1) sin(o1)*cos(k1)+cos(o1)*sin(p1)*sin(k1);
sin(p1) -sin(o1)*cos(p1) cos(o1)*cos(p1)];
M2=[cos(p2)*cos(k2) cos(o2)*sin(k2)+sin(o2)*sin(p2)*cos(k2) sin(o2)*sin(k2)-cos(o2)*sin(p2)*cos(k2);
-cos(p2)*sin(k2) cos(o2)*cos(k2)-sin(o2)*sin(p2)*sin(k2) sin(o2)*cos(k2)+cos(o2)*sin(p2)*sin(k2);
sin(p2) -sin(o2)*cos(p2) cos(o2)*cos(p2)];
X_init=zeros(m,1);
Y_init=zeros(m,1);
Z_init=zeros(m,1);
result=zeros(m,3);
for i=1:m
u1=M1(1,1)*x1(i,1)+M1(2,1)*y1(i,1)+M1(3,1)*-focal;
v1=M1(1,2)*x1(i,1)+M1(2,2)*y1(i,1)+M1(3,2)*-focal;
w1=M1(1,3)*x1(i,1)+M1(2,3)*y1(i,1)+M1(3,3)*-focal;
C1=%%% You can find the equations for intersecction from the Lab slide
C2=%%%
u2=M2(1,1)*x2(i,1)+M2(2,1)*y2(i,1)+M2(3,1)*-focal;
v2=M2(1,2)*x2(i,1)+M2(2,2)*y2(i,1)+M2(3,2)*-focal;
w2=M2(1,3)*x2(i,1)+M2(2,3)*y2(i,1)+M2(3,3)*-focal;
C3=%%%
C4=%%%
B=%%%
f=%%%
W=eye(4,4);
N=B'*W*B;
t=B'*W*f;
del=inv(N)*t;
X0=%%% initial approximation of X, Y, Z are the elements of del.
Y0=%%%
Z0=%%%
L=[x1(i,1);y1(i,1);x2(i,1);y2(i,1)];
L0=L;
par_0=[X0;Y0;Z0];
keep_going=1;
last_phi=10;
iter=0;
while keep_going==1
syms xx1 yy1 xx2 yy2 X Y Z
U1=M1(1,1)*xx1+M1(2,1)*yy1+M1(3,1)*-focal;
V1=M1(1,2)*xx1+M1(2,2)*yy1+M1(3,2)*-focal;
W1=M1(1,3)*xx1+M1(2,3)*yy1+M1(3,3)*-focal;
U2=M2(1,1)*xx2+M2(2,1)*yy2+M2(3,1)*-focal;
V2=M2(1,2)*xx2+M2(2,2)*yy2+M2(3,2)*-focal;
W2=M2(1,3)*xx2+M2(2,3)*yy2+M2(3,3)*-focal;
FF=[(X-XL1)-(Z-ZL1)*U1/W1;
(Y-YL1)-(Z-ZL1)*V1/W1;
(X-XL2)-(Z-ZL2)*U2/W2;
(Y-YL2)-(Z-ZL2)*V2/W2];
BB=%%% use jacobian() function
AA=%%% use jacobian() function
X=par_0(1,1);
Y=par_0(2,1);
Z=par_0(3,1);
xx1=L0(1,1);
yy1=L0(2,1);
xx2=L0(3,1);
yy2=L0(4,1);
F=%%% evaluate FF using eval() function
B=%%% evaluate BB using eval() function
A=%%% evaluate AA using eval() function
Q=inv(W);
Qe=A*Q*A';
We=inv(Qe);
f=%
N=%
t=%
delta=%
v=%
phi=v'*W*v;
obj=abs((last_phi-phi)/last_phi)
if (obj<0.0001)
keep_going=0
disp('we have converged');
end
L0=L+v;
par_0=par_0+delta;
if (obj<0.0001)
result(i,:)=par_0;
end
if (iter>100)
keep_going=0;
disp('too many iterations');
end
last_phi=phi;
iter=iter+1;
end
end
Answers (0)
Categories
Find more on MATLAB in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!