Help with initial approximation of parameters [X Y Z] for intersection by linear method

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

Products

Release

R2022a

Tags

Asked:

on 25 Oct 2023

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!