讨论opencv位姿估计结果与实际运动轨迹的关系（涉及变换矩阵与其逆矩阵）

讨论opencv位姿估计结果与实际运动的关系（涉及变换矩阵与其逆矩阵）

本文将讨论对于三维空间中的同一组点，由两个不同位置的观测数据（相机模型下）来求解其坐标变换以及观测位置运动的数学原理及相关c++程序实现：

---

一.两个观测数据与坐标变换

假设对于空间中一组三维点P，我们得到了其在A、B两点的相机观测数据m_A、m_B，其中A处模型为以A为光心的针孔模型，B处模型为以相机左上角为原点，向右、向下延伸坐标轴的像素模型，m_A为三维点P在A处模型下的三维坐标 ( x , y , z )，m_B为三维点P在B处模型下的成像平面坐标 ( u , v )。

opencv中的solvePnPRansac函数原型如下：

CV_EXPORTS_W void solvePnPRansac( 
                        InputArray objectPoints,
                        InputArray imagePoints,
                        InputArray cameraMatrix,
                        OutputArray rvec,
                        OutputArray tvec,
                        bool useExtrinsicGuess = false,
                        int iterationsCount = 100,
                        float reprojectionError = 8.0,
                        int minInliersCount = 100,
                        OutputArray inliers = noArray(),
                        int flags = ITERATIVE);

• 输入：参数1、2分别使用观测数据m_A、m_B，参数3为相机内参矩阵（需要fx、fy、cx、cy），参数7为迭代计算次数
• 输出：参数4、5、10为输出部分，其中参数4、5对应由姿态A变换到姿态B过程的旋转向量（rotate vector）以及平移向量（translation vector）
• 参数10猜想为符合所求旋转平移向量的点对

如你所见，当我们给出我们的观测数据以及相机内参矩阵后，可以得到旋转向量rvec和平移向量tvec，围绕这两个向量进行运算又能够构造两个模型间的变换矩阵T，在三维空间齐次坐标下，变换矩阵为4*4矩阵，形式如下：

$$T= \begin{pmatrix} R_{3*3} & t_{3*1} \\ 0 & 1 \\ \end{pmatrix}$$
事实上，这样一个变换矩阵可以完成这样的事情：
依据前文的假设，对于P点在A处的观测数据m _A( x ₁ , y ₁ , z ₁ )，我们将其写作齐次坐标的列向量形式：
$$M_A = \begin{pmatrix} x_1 \\ y_1 \\ z_1 \\ 1 \\ \end{pmatrix}$$
则P点在B处观测到的三维坐标（齐次坐标列向量形式）为：