在 matlab 中通过 Levenberg-Marquardt 算法解决仅旋转的绝对方向

Question

我正在做迭代最近点项目。有称为 "a" 和 "b" 的点集。我想找到适合 "a" 和 "b" 的变换矩阵。 SVD可以完美求解，得到旋转和平移矩阵。现在我考虑制作一些仅旋转的关节，拟合 "a" 和 "b" 点集，没有任何平移，只有旋转。我在网上搜索了一下，有的讨论说Levenberg-Marquardt算法可以解决。

我这里复制代码，修改代码为绝对方向问题的代价函数https://engineering.purdue.edu/kak/computervision/ECE661/HW5_LM_handout.pdf

成本函数为

E=Σ|| Ra-b-t ||^2

R 是旋转矩阵，t 是平移矩阵

matlab 代码如下，它将return 最佳旋转弧度"R" 和平移"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

它和关闭形式的解决方案一样有效 SVD.Now 我不想翻译，我认为公式是

E=Σ|| Ra-b ||^2

这意味着我只旋转 "a" 并使 "b" 适合原点。

。代码如下，它将 return 最佳旋转弧度 "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

在这段代码中，我删除了成本函数的平移矩阵，然后进行 Levenberg-Marquardt 算法，我希望它会 return 适合 "a" 和 [=36 的最佳旋转矩阵=] 点集。但是，它总是 return 初始猜测 r0。看来我不能简单地删除成本函数中的平移矩阵来获得最佳旋转。

我应该怎么做才能解决这个只能旋转的绝对方向问题？谢谢你的想法！

Answer 1

您也可以通过 SVD 执行此操作。如果你想要旋转和平移，第一步是从 a 和 b 中减去均值。如果您只需要旋转，则可以省略此步骤，像以前一样计算旋转并错过计算平移的最后一步。

在 matlab 中通过 Levenberg-Marquardt 算法解决仅旋转的绝对方向

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

matlab

least-squares

levenberg-marquardt