一种简单的位姿估计（PoseEstimation）计算方法

尊重作者脑力和体力劳动，转载请注明出处，谢谢。

位姿估计是计算机视觉领域一个基本问题。若已获得合作标志在摄像机坐标系下的坐标值，一般情况可采用奇异值分解或最小二乘的方法求解位姿信息中的旋转矩阵与平移矩阵，满足

Pci=RcwPwi+t0 P_i^c=R_w^cP_i^w+t_0

R2w=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥,t0=⎡⎣⎢txtytz⎤⎦⎥
R_w^2= \begin{bmatrix}
r_{11} &r_{12}& r_{13} \\
r_{21} & r_{22} & r_{23} \\
r_{31}& r_{32} & r_{33}\\
\end{bmatrix},t_0=
\begin{bmatrix}
t_x \\
t_y\\
t_z
\end{bmatrix}


其中下标cc表示CameraCamera，ww表示WorldWorld。

奇异值分解是很通用的一种算法，但还不够简单。

一般情况下得到33个特征点就可以获得位姿信息。不失一般性我们假设合作目标上共有nn个标志，其在物体自身坐标系下的坐标为

Pwi,i=1,2,…，nP_i^w ,i=1,2,…，n

计算得到对应的特征点在摄像机坐标系下的坐标为

P^ci,i=1,2,…，n \hat P _i^c,i=1,2,…，n（ P^ci\hat P_i^c表示计算值。）

在物体坐标系下，合作标志中心点坐标应满足

Pwcenter=∑i=1nωiPwi,,i=1,2,……，n P_{center} ^w =\sum_{i=1}^n\omega _i P_i^w, ,i=1,2,……，n

系数ωi \omega _i 在设计合作标志时可精确得到，在已知各系数ωi \omega _i 时，可得到合作标志中心在摄像机坐标系的坐标为

P^ccenter=∑i=1nωiP^ci,,i=1,2,……，n\hat P_{center} ^c = \sum_{i=1}^n\omega _i \hat P_i^c, ,i=1,2,……，n

这就是合作目标中心的位置信息。特殊地，当合作标志特征点布置关于中心点对称且均匀分布时，位置信息为

P^ccenter=1n∑i=1nP^ci,,i=1,2,……，n\hat P_{center} ^c =\frac{1}{n} \sum_{i=1}^n \hat P_i^c, ,i=1,2,……，n

姿态信息求解。




如图11所示，当合作标志特征点个数为33时，且33点不共线时，记

α1=Pw1Pw2→ \alpha _1= \vec{P_1^wP_2^w}

α2=Pw1Pw3→ \alpha _2= \vec{P_1^wP_3^w}

α3=α1×α2 \alpha _3= \alpha _1 \times \alpha _2

同样，在解算获得的坐标中建立

β1=P^c1P^c2→ \beta _1= \vec{\hat P_1^c \hat P_2^c}

β2=P^c1P^c3→ \beta _2= \vec{\hat P_1^c \hat P_3^c}

β3=β1×β2 \beta _3= \beta _1 \times \beta _2

设摄像机坐标系到物体坐标系的旋转矩阵为RcwR_w^c ，则满足

βi=Rcwαi,i=1,2,3 \beta _i=R_w^c \alpha _i,i=1,2,3

记

A=[α1α2α3],B=[β1β2β3]
A= \begin{bmatrix}
\alpha _1 &\alpha _2& \alpha _3
\end{bmatrix},B=
\begin{bmatrix}
\beta _1& \beta _2& \beta _3
\end{bmatrix}


得到

B=RcwA B=R_w^c A

故

Rcw=BA−1 R_w^c=B A^{-1}

现证明A−1A^{-1}存在。

假设A−1A^{-1}不存在，则rank(A)<3rank(A) <3，那么存在一组不全为00的数{m1,m2,m3} \{m_1,m_2,m_3\}使得m1α1+m2α2+m3α3=0m_1\alpha_1+m_2\alpha_2+m_3\alpha_3=0成立。当m3=0m_3=0时，即m1α1+m2α2=0m_1\alpha_1 +m_2\alpha_2=0,即α1,α2\alpha_1 ,\alpha_2共线，即3个特征点共线，这与初始条件不符，故m3≠0m_3 \neq 0。WhenWhen m3≠0m_3 \neq 0则α3=1m3(m1α1+m2α2)\alpha_3=\frac{1}{m_3}(m_1\alpha_1 +m_2\alpha_2),这违背α3=α1×α2 \alpha _3= \alpha _1 \times \alpha _2。假设不成立。故当3特征点不共线时，A−1A^{-1}存在。

当特征点个数为nn时，可将其分为mm组3点集，对每组求得 RcwR_w^c,记做RiR_i

最终取

Rcw=1m∑i=1mRi,i=1,2…，mR_w^c=\frac{1}{m} \sum_{i=1}^m R_i,i=1,2…，m

这样就获得了相对姿态信息。

优化：

1)在矩阵计算时，求逆运算比较麻烦，可通过设

α4=α2×α3 \alpha _4= \alpha _2 \times \alpha _3

β4=β2×β3 \beta _4= \beta _2 \times \beta _3

A=[α4α2α3],B=[β4β2β3]
A= \begin{bmatrix}
\alpha _4 &\alpha _2& \alpha _3
\end{bmatrix},B=
\begin{bmatrix}
\beta _4& \beta _2& \beta _3
\end{bmatrix}

将两个坐标系转换成正交坐标系，这样就可以用ATA^T代替A−1A^{-1},避免矩阵求逆，大大提高运算效率。

2) 姿态信息用欧拉角时，用平均值代表真实值会产生较大误差，因为三角函数其本身非线性，因此在求平均前先将其转换到四元数空间，再进行平均，之后再转化到欧拉角，这样更加精确。

Matlab算法如下：

[code]///////////////////////////////////////////////////////////
// Company: NWPU
// Engineer: LamberJee
// Create Date:    16:16:21 03/21/2015 
// Design Name:   A method for PoseEstimation
// Tool versions: Matlab-R2013a
// Description: 
// Revision: beta_0.01
// Additional Comments: 
///////////////////////////////////////////////////////////

function [Rp,tp] = CaculPose(Camerb,Object)
    XXc = Camerb;
    XXw = Object;
    n = size(XXw,2);
    Q = zeros(4,1);
    for i = 1:n
        if i == 1 || i == 2
            continue
        end
        a1 = XXw(:,1)-XXw(:,i);
        a2 = XXw(:,2)-XXw(:,i);
        a3 = xcross(a1,a2);
        a4 = xcross(a2,a3);

        a2 = a2/norm(a2);
        a3 = a3/norm(a3);
        a4 = a4/norm(a4);
        A = [a4 a2 a3];

        b1 = XXc(:,1)-XXc(:,i);
        b2 = XXc(:,2)-XXc(:,i);
        b3 = xcross(b1,b2);
        b4 = xcross(b2,b3);

        b2 = b2/norm(b2);
        b3 = b3/norm(b3);
        b4 = b4/norm(b4);
        B = [b4 b2 b3];

        Ri = B * A';
        Qi = quater(Ri);
        Q = Q + Qi;
    end
    Qp = Q /(n-2);
    Rp = Eler(Qp);

    t = zeros(3,1);
    for j = 1:n
        t = t + XXc(:,j);
    end
    tp = t / n;
return
function c = xcross(a,b)
    c = [a(2)*b(3)-a(3)*b(2);
        a(3)*b(1)-a(1)*b(3);
        a(1)*b(2)-a(2)*b(1)];
 return

 function Q = quater(R)
    r11 = R(1); r12 = R(4);r13 = R(7);
    r21 = R(2); r22 = R(5);r23 = R(8);
    r31 = R(3); r32 = R(6);r33 = R(9);

    q1 = 0.5*sqrt(1+r11+r22+r33);
    q2 = 0.25*(r32-r23)/q1;
    q3 = 0.25*(r13-r31)/q1;
    q4 = 0.25*(r21-r12)/q1;

    Q = [q1 q2 q3 q4]';   
 return

 function  R = Eler(Q)
     q0 = Q(1);q1 = Q(2);q2 = Q(3);q3 = Q(4);
     r11 = (q0)^2+(q1)^2-(q2)^2-(q3)^2;
     r12 = 2*(q1*q2-q0*q3);
     r13 = 2*(q1*q3+q0*q2);
     r21 = 2*(q1*q2+q0*q3);
     r22 = (q0)^2-(q1)^2+(q2)^2-(q3)^2;
     r23 = 2*(q2*q3-q0*q1);
     r31 = 2*(q1*q3-q0*q2);
     r32 = 2*(q0*q1+q2*q3);
     r33 = (q0)^2-(q1)^2-(q2)^2+(q3)^2;

     R = [r11 r12 r13;
          r21 r22 r23;
          r31 r32 r33];

 return