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

---

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

---

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

$$P_i^c=R_w^cP_i^w+t_0$$
$$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}$$
其中下标 $c$表示 $Camera$， $w$表示 $World$。
奇异值分解是很通用的一种算法，但还不够简单。
一般情况下得到 $3$个特征点就可以获得位姿信息。不失一般性我们假设合作目标上共有 $n$个标志，其在物体自身坐标系下的坐标为
$$P_i^w ,i=1,2,…，n$$
计算得到对应的特征点在摄像机坐标系下的坐标为
$$\hat P _i^c,i=1,2,…，n$$ （