一种简单的位姿估计（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;returnfunction 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