讨论opencv位姿估计结果与实际运动的关系(涉及变换矩阵与其逆矩阵)
本文将讨论对于三维空间中的同一组点,由两个不同位置的观测数据(相机模型下)来求解其坐标变换以及观测位置运动的数学原理及相关c++程序实现:
一.两个观测数据与坐标变换
假设对于空间中一组三维点P,我们得到了其在A、B两点的相机观测数据mA、mB,其中A处模型为以A为光心的针孔模型,B处模型为以相机左上角为原点,向右、向下延伸坐标轴的像素模型,mA为三维点P在A处模型下的三维坐标 ( x , y , z ),mB为三维点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分别使用观测数据mA、mB,参数3为相机内参矩阵(需要fx、fy、cx、cy),参数7为迭代计算次数
- 输出:参数4、5、10为输出部分,其中参数4、5对应由姿态A变换到姿态B过程的旋转向量(rotate vector)以及平移向量(translation vector)
- 参数10猜想为符合所求旋转平移向量的点对
如你所见,当我们给出我们的观测数据以及相机内参矩阵后,可以得到旋转向量rvec和平移向量tvec,围绕这两个向量进行运算又能够构造两个模型间的变换矩阵T,在三维空间齐次坐标下,变换矩阵为4*4矩阵,形式如下:
T=(R3∗30t3∗11)
事实上,这样一个变换矩阵可以完成这样的事情:
依据前文的假设,对于P点在A处的观测数据m A( x 1 , y 1 , z 1 ),我们将其写作齐次坐标的列向量形式:
MA=⎛⎝⎜⎜⎜x1y1z11⎞⎠⎟⎟⎟
则P点在B处观测到的三维坐标(齐次坐标列向量形式)为: