solving rotation-only absolute orientation by Levenberg-Marquardt algorithm in matlab

Question

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-bt ||^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!

Answer 1

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.