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

---

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

---

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

Pci=RcwPwi+t0

R2w=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥,t0=⎡⎣⎢txtytz⎤⎦⎥

其中下标 c 表示 Camera ， w 表示 World 。
奇异值分解是很通用的一种算法，但还不够简单。
一般情况下得到 3 个特征点就可以获得位姿信息。不失一般性我们假设合作目标上共有 n 个标志，其在物体自身坐标系下的坐标为

Pwi,i=1,2,…，n

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

P^ci,i=1,2,…，n

（ P^ci 表示计算值。）
在物体坐标系下，合作标志中心点坐标应满足

Pwcenter=∑i=1nωiPwi,,i=1,2,……，n

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

P^ccenter=∑i=1nωiP^ci,,i=1,2,……，n

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

P^ccenter=1n∑i=1nP^ci,,i=1,2,……，n

姿态信息求解。

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

α1=Pw1Pw2→

α2=Pw1Pw3→

α3=α1×α2

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

β1=P^c1P^c2→

β2=P^c1P^c3→

β3=β1×β2

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

βi=Rcwαi,i=1,2,3

记

A=[α1α2α3],B=[β1β2β3]

得到

B=RcwA

故

Rcw=BA−1

现证明 A−1 存在。
假设 A−1 不存在，则 rank(A)<3 ，那么存在一组不全为 0 的数 {m1,m2,m3} 使得 m1α1+m2α2+m3α3=0 成立。当 m3=0 时，即 m1α1+m2α2=0 ,即 α1,α2 共线，即3个特征点共线，这与初始条件不符，故 m3≠0 。 When m3≠0 则 α3=1m3(m1α1+m2α2) ,这违背 α3=α1×α2 。假设不成立。故当3特征点不共线时， A−1 存在。
当特征点个数为 n 时，可将其分为 m 组3点集，对每组求得 Rcw ,记做 Ri
最终取

Rcw=1m∑i=1mRi,i=1,2…，m

这样就获得了相对姿态信息。
优化：
1)在矩阵计算时，求逆运算比较麻烦，可通过设

α4=α2×α3

β4=β2×β3

A=[α4α2α3],B=[β4β2β3]

将两个坐标系转换成正交坐标系，这样就可以用 AT 代替 A−1 ,避免矩阵求逆，大大提高运算效率。
2) 姿态信息用欧拉角时，用平均值代表真实值会产生较大误差，因为三角函数其本身非线性，因此在求平均前先将其转换到四元数空间，再进行平均，之后再转化到欧拉角，这样更加精确。

Matlab算法如下：

///////////////////////////////////////////////////////////
// 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