三维姿态：关于solvePnP与cvPOSIT

之所以写：

场景：给定物体3D点集与对应的图像2D点集，之后进行姿态计算(即求旋转与位移矩阵)。

在翻阅opencv api时看到这2个函数输出都是旋转与位移，故做简单分析并记录于此。

官方解释：

solvePnP(http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp)

Finds an object pose from 3D-2D point correspondences.

bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec,bool useExtrinsicGuess=false, int flags=ITERATIVE )

cvPOSIT(http://www.opencv.org.cn/index.php/Cv%E7%85%A7%E7%9B%B8%E6%9C%BA%E5%AE%9A%E6%A0%87%E5%92%8C%E4%B8%89%E7%BB%B4%E9%87%8D%E5%BB%BA#POSIT)

执行POSIT算法

void cvPOSIT( CvPOSITObject* posit_object, CvPoint2D32f* image_points, double focal_length, CvTermCriteria criteria, CvMatr32f rotation_matrix,  CvVect32f translation_vector );

理解：

相同点：1.输入都是3D点集和对应的2D点集，其中cvPOSIT的3D点包含在posit_object结构中

               2.输出均包括旋转矩阵和位移向量

不同点：solvePnP有摄像机的一些内参

solvePnP源码：

 

void cv::solvePnP( InputArray _opoints, InputArray _ipoints,
                  InputArray _cameraMatrix, InputArray _distCoeffs,
                  OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess )
{
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
    
    _rvec.create(3, 1, CV_64F);
    _tvec.create(3, 1, CV_64F);
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
    CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
    CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
    CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
    cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                                 c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
                                 &c_rvec, &c_tvec, useExtrinsicGuess );
}

结论：可以看到，除了前面的一堆数据类型检查和转化外，其实solvePnP调用的是cvFindExtrinsicCameraParams2通过已知的内参进行未知外参求解，是一个精确解；而cvPOSIT是用仿射投影模型近似透视投影模型下，不断迭代计算出来的估计值(在物体深度变化相对于物体到摄像机的距离比较大的时候,这种算法可能不收敛)。

 

 

3d坐标->2d坐标，毫米到像素的转换主要体现在fx、fy、cx、cy上，这些量均以像素为单位来表示实际物理长度(mm)。而单位像素有多少个毫米，受sensor面板感光颗粒(单个物理像素)尺寸影响。

 

solvePnP输出的rvec是旋转向量，可以通过Rodrigues转换成旋转矩阵，有需要可以再转到欧拉角：

static Vec3f rotationMatrixToEulerAngles(Mat &R)
{
    float sy = sqrt(R.at(0,0) * R.at(0,0) +  R.at(1,0) * R.at(1,0) );

    bool singular = sy < 1e-6; // If

    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at(2,1) , R.at(2,2));
        y = atan2(-R.at(2,0), sy);
        z = atan2(R.at(1,0), R.at(0,0));
    }
    else
    {
        x = atan2(-R.at(1,2), R.at(1,1));
        y = atan2(-R.at(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z)*180/3.14159;
}

void test()
{
    ...
    Mat Rv,R,T;
    cv::solvePnP(objPts,imgPts,cameraMatrix,Mat(),Rv,T);
    //旋转向量转旋转矩阵
    Rodrigues(Rv,R);
    //旋转矩阵转欧拉角
    Vec3f angles = rotationMatrixToEulerAngles(R);
    ...

}