I'm doing iterative closest point project. There are point set called "a" and "b".I want to find the transformation matrix to fit "a" and "b". the SVD can solve it perfectly and get the rotation and translation matrix. now I consider making some rotation-only joints, fitting the "a" and "b" point sets without any translation but only rotation. I searched the internet and some discussion said Levenberg-Marquardt algorithm can solve it.
I copy the code here and modify the code to the cost function of absolute orientation problem https://engineering.purdue.edu/kak/computervision/ECE661/HW5_LM_handout.pdf
and the cost function is
E=Σ|| Ra-b-t ||^2
R would be the Rotation matrix and t would be the translation matrix
the matlab code is below, It would return best rotation radian "R" and translation "t":
a=[0 1 2 3 4 5 6 7 8 9;0 0 0 0 0 0 0 0 0 0];
b=[0 0.7074 1.4148 2.1222 2.8296 3.5369 4.2443 4.9517 5.6591 6.3665;0 -0.7068 -1.4137 -2.1205 -2.8273 -3.5341 -4.2410 -4.9478 -5.6546 -6.3614];
r0=0.0;
tx0=0;
ty0=0;
y_init = [cos(r0) -sin(r0);sin(r0) cos(r0)]*a-[tx0;ty0]*[1 1 1 1 1 1 1 1 1 1];
Ndata=length(b);
Nparams=3;
n_iters=50;
lamda=0.01;
updateJ=1;
r_est=r0;
tx_est=tx0;
ty_est=ty0;
for it=1:n_iters
if updateJ==1
J=zeros(Ndata*2,Nparams);
for i=0:length(a)-1
J(2*i+1,:)= [- a(2,i+1)*cos(r_est) - a(1,i+1)*sin(r_est), -1, 0];
J(2*i+2,:)= [ a(1,i+1)*cos(r_est) - a(2,i+1)*sin(r_est), 0, -1];
end
y_est = [cos(r_est) -sin(r_est);sin(r_est) cos(r_est)]*a-[tx_est;ty_est]*[1 1 1 1 1 1 1 1 1 1];
d=b-y_est;
H=J'*J;
if it==1
e=dot(d,d);
end
end
H_lm=H+(lamda*eye(Nparams,Nparams));
dp=inv(H_lm)*(J'*d(:));
H_lm;
inv(H_lm);
J'*d(:);
g = J'*d(:);
r_lm=r_est+dp(1);
tx_lm=tx_est+dp(2);
ty_lm=ty_est+dp(3);
y_est_lm = [cos(r_lm) -sin(r_lm);sin(r_lm) cos(r_lm)]*a-[tx_lm;ty_lm]*[1 1 1 1 1 1 1 1 1 1];
d_lm=b-y_est_lm;
e_lm=dot(d_lm,d_lm);
if e_lm<e
lamda=lamda/10;
r_est=r_lm;
tx_est=tx_lm;
ty_est=ty_lm;
e=e_lm;
updateJ=1;
else
updateJ=0;
lamda=lamda*10;
end
end
r_est
It worked as well as close form solution like SVD.Now I don't want the translation,I think the formula would be
E=Σ|| Ra-b ||^2
this means I only rotate the "a" and fit the "b" around origin point.
. The code would be below, It would return best rotation radian "R":
a=[0 1 2 3 4 5 6 7 8 9;1 1 1 1 1 1 1 1 1 1];
b=[-1 -2 -3 -4 -5 -6 -7 -8 -9 -10;0 0 0 0 0 0 0 0 0 0];
%initial guess
r0=0;
y_init = [cos(r0) -sin(r0);sin(r0) cos(r0)]*a;
Ndata=length(b);
Nparams=1;
n_iters=50;
lamda=0.01;
updateJ=1;
r_est=r0;
for it=1:n_iters
if updateJ==1
J=zeros(Ndata,Nparams);
for i=0:length(a)-1
J(2*i+1,:)= [-a(2,i+1)*cos(r_est)-a(1,i+1)*sin(r_est)];
J(2*i+2,:)= [ a(1,i+1)*cos(r_est)-a(2,i+1)*sin(r_est)];
end
y_est = [cos(r_est) -sin(r_est);sin(r_est) cos(r_est)]*a;
d=b-y_est;
H=J'*J;
if it==1
e=dot(d,d);
end
end
H_lm=H+(lamda*eye(Nparams,Nparams));
dp=inv(H_lm)*(J'*d(:));
H_lm;
inv(H_lm);
J'*d(:);
g = J'*d(:);
r_lm=r_est+dp(1);
y_est_lm = [cos(r_lm) -sin(r_lm);sin(r_lm) cos(r_lm)]*a;
d_lm=b-y_est_lm;
e_lm=dot(d_lm,d_lm);
if e_lm<e
lamda=lamda/10;
r_est=r_lm;
e=e_lm;
updateJ=1;
else
updateJ=0;
lamda=lamda*10;
end
end
r_est
in this code, I delete the translation matrix of the cost function then do the Levenberg-Marquardt algorithm, I hope it would return the best rotation matrix that fit "a" and "b" point sets. However, it always return the initial guess r0. It seems that I cannot simply delete the translation matrix in the cost function to get the best rotation.
What should I do to solve this rotation-only absolute orientation problem? thanks for any idea!
You can do this via SVD too. If you want the rotation and translation the first step is to subtract the means from the a's and the b's. If you want just the rotation you omit this step, compute the rotation as before and miss out the final step of computing the translation.