cv::recoverPose()函数

在OpenCV中cv::recoverPose()函数主要是用来从本质矩阵中恢复 R , t R,t R,t

该函数在OpenCV中一共有以下这几种形式：

int recoverPose( InputArray E, InputArray points1, InputArray points2,
                 InputArray cameraMatrix, OutputArray R, OutputArray t,
                 InputOutputArray mask = noArray() );

int recoverPose( InputArray E, InputArray points1, InputArray points2,
                 OutputArray R, OutputArray t, double focal = 1.0,
                 Point2d pp = Point2d(0, 0), InputOutputArray mask = noArray() );

int recoverPose( InputArray E, InputArray points1, InputArray points2,
                 InputArray cameraMatrix, OutputArray R, OutputArray t, 
                 double distanceThresh, InputOutputArray mask = noArray(),
                 OutputArray triangulatedPoints = noArray());

参数介绍：
　　E：已经求解出来的本质矩阵，它是3x3的矩阵；
　　points1：第一张图片中的点；
　　points2：第二张图片中的点；
　　cameraMatrix：相机内参矩阵，它是3x3的矩阵；
　　R：求解出来的两帧图片之间的旋转矩阵；
　　t：求解出来的两帧图片之间的平移向量；
　　focal：相机焦距；
　　pp：像素坐标的原点；
　　distanceThresh：点的距离阈值，用来滤出距离较远的点；
　　triangulatedPoints：通过三角化还原点；

注意：
　　1. 通过该函数求解出来的 R , t R,t R,t，它表示的是points1到points2的变换，也就是 R 1 t o 2 R_{1to2} R1to2​， t 1 t o 2 t_{1to2} t1to2​
　　2.该函数求解出来的 R , t R,t R,t已经是最合适的 R , t R,t R,t，已经通过内部的代码去掉了另外三种错误的解
　　3. cv::recoverPose()中points1和points2的输入顺序，必须也要和求本质矩阵时对函数cv::findEssentialMat()输入的顺序相同（可以参考如下例子）

代码示例

//求解两张图片对应的本质矩阵
cv::Mat essential_matrix = cv::findEssentialMat(points1, points2, K,  cv::RANSAC);

//从本质矩阵中恢复R,t
cv::Mat R;
cv::Mat t;
cv::recoverPose(essential_matrix, points1, points2, K, R, t);

Open-VINS代码示例

bool MotionEstimator::solveRelativeRT(const std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &corres,
                                      Eigen::Matrix3d &Rotation, Eigen::Vector3d &Translation)
{
    // at least 15 corresponding point pairs
    if (corres.size() >= 15)
    {
        std::vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }
        cv::Mat mask;
        // param1 : ransacReprojThreshold
        cv::Mat F = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
        cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
        cv::Mat rot, trans;
        int inlier_cnt = cv::recoverPose(F, ll, rr, cameraMatrix, rot, trans, mask);

        Eigen::Matrix3d R;
        Eigen::Vector3d T;
        for (int i = 0; i < 3; i++)
        {
            T(i) = trans.at<double>(i, 0);
            for (int j = 0; j < 3; j++)
                R(i, j) = rot.at<double>(i, j);
        }

        Rotation = R.transpose();
        Translation = -R.transpose() * T;
        // at least has 12 inliers
        if(inlier_cnt > 12)
            return true;
        else
            return false;
    }
    return false;
}