c++ opencv cv::solvePnPRansac 的使用和注意事项

最近在做关于RGBD人脸拟合的事情,遇到一个关于 solvePnPRansac 使用的问题,记录一下. 顺便记录几个函数,里面包含了自己对 eigen 使用的一些尝试. 终于对eigen由陌生到可以较为熟练地使用了.

问题描述

一开始的现象是同样的参数使用solvePnP是正确的,而使用solvePnPRansac 就有问题,报错如下

terminate called after throwing an instance of 'cv::Exception'
  what():  OpenCV(3.4.5) /data/soft/opencv-3.4.5/modules/core/src/matrix_wrap.cpp:1235: error: (-215:Assertion failed) !fixedSize() || ((Mat*)obj)->size.operator()() == Size(_cols, _rows) in function 'create'

开始挺摸不着头脑的, 猜测是传入的数据类型不对,后来才知道 rvec, tvec 不能指定类型 (在solvePnp中是可以的)

利用solve pnp求解到相机姿态下的方法

// 这个是针对已经把相机内参去掉的p2d
void Calc_perspective_pm(float * pm, int n, const float * p3d, const float *p2d, int type){
    const cv::Mat1f noK = cv::Mat1f::eye(3, 3); // 去掉相机内参之后,就是单位阵了

    const cv::Mat1f noK_dst(n, 2, const_cast<float *>(p2d));
    const cv::Mat1f src(n, 3, const_cast<float *>(p3d));
    //auto noK_dst = cv::Mat1f(n, 2, p2d);

    //cv::Mat1f rvec, tvec;  这个对于 ransac 不行
    cv::Mat rvec, tvec;
    cv::Mat1f distCoeffs = cv::Mat1f::zeros(1, 5);
    if(type==0){
        cv::solvePnP(src, noK_dst, noK, distCoeffs, rvec, tvec);
    }else if(type==1){
        //cv::solvePnPRansac(cv::Mat(src), cv::Mat(noK_dst), cv::Mat(noK), cv::Mat(distCoeffs), rvec, tvec);
        cv::solvePnPRansac(src, noK_dst, noK, distCoeffs, rvec, tvec,
                false, 100, 8.0, 0.99, cv::noArray(), cv::SOLVEPNP_P3P);

    }else{
        cout<<"ERR: Calc_perspective_pm not support type: "<<type<<endl;
        exit(1);
    }

    //@7-10
    //cout<<rvec.type()<<" "<<src.type()<<endl;
    if(rvec.type()==5){
        memcpy(pm, rvec.ptr<float>(), 3*sizeof(float));
        memcpy(pm+3, tvec.ptr<float>(), 3*sizeof(float));
    }else if(rvec.type()==6){ // after solvePnPRansac
        memcpy(pm, rvec.ptr<double>(), 3*sizeof(double));
        memcpy(pm+3, tvec.ptr<double>(), 3*sizeof(double));
    }else{
        cout<<"ERR: Calc_perspective_pm, something wrong in ocv "<<endl;
        exit(1);
    }
    /*
    如果要求相机在世界坐标系下的位置
cv::Mat R;
cv::Rodrigues(rvec, R); // R is 3x3

R = R.t();  // rotation of inverse
tvec = -R * tvec; // translation of inverse

cv::Mat T = cv::Mat::eye(4, 4, R.type()); // T is 4x4
T( cv::Range(0,3), cv::Range(0,3) ) = R * 1; // copies R into T
T( cv::Range(0,3), cv::Range(3,4) ) = tvec * 1; // copies tvec into T
    */

}

去除相机参数 (use eigen)

上面我给的函数需要提前把相机内参给去掉,这里我把这个函数提供出来,同时也是对 eigen 矩阵操作的记录

// type: 0: 只有四个数, 且不是矩阵; 1: 只有四个数且是矩阵; 2: 按照2*3 矩阵来算
void Calc_remove_camK(int n, float *p2_5d, const float *p2d, const float *camK, int type) {
    using namespace Eigen;
    using namespace define_utils;

    MapMatXf P2_5d(p2_5d, 2, n);
    constMapMatXf P2d(p2d, 2, n);

    assert(type<3);
    if(type==2){
        MatrixXf inv_K2x2(2, 2);
        VectorXf txy(2);

        txy[0] = camK[2];
        txy[1] = camK[3+2];
        float ad_bc = camK[0]*camK[3+1] - camK[1]*camK[3];

        inv_K2x2(0, 0) = camK[3+1];
        inv_K2x2(0, 1) = -camK[1];
        inv_K2x2(1, 0) = -camK[3];
        inv_K2x2(1, 1) = camK[0];
        inv_K2x2 /= ad_bc;

        P2_5d = inv_K2x2 * P2d;
        P2_5d.colwise() -= inv_K2x2 * txy;
    }else{
        VectorXf fxy(2), txy(2);
        if(type==0){
            fxy(0) = camK[0];
            fxy(1) = camK[2];
            txy(0) = camK[1];
            txy(1) = camK[3];
        }else{
            fxy(0) = camK[0];
            fxy(1) = camK[3+1];
            txy(0) = camK[2];
            txy(1) = camK[3+2];
        }

        if(p2_5d!=p2d) P2_5d = P2d;
        P2_5d.colwise() -= txy;

        P2_5d.block(0, 0, 1, n) /= fxy(0);
        P2_5d.block(1, 0, 1, n) /= fxy(1);

    }

}

透视投影函数 (use eigen)

用了一些 eigen的矩阵操作技巧记录一下

// 自己的简化定义
namespace define_utils{
    using c_F = float;
    using c_Fp = float*;
    using c_cFp = const float*;
    using c_cIp = const int*;
    using c_uint32 = unsigned int;

    using MapVecXf = Eigen::Map<Eigen::VectorXf>;
    using constMapVecXf = Eigen::Map<const Eigen::VectorXf>;
    using MapMatXf = Eigen::Map<Eigen::MatrixXf>;
    using constMapMatXf = Eigen::Map<const Eigen::MatrixXf>;

    // dynamic = -1
    using constMapMatXf_RM = Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >;

}


// 里面注释掉的都是我想的eigen操作方式,但是不成功
void Calc_project3D_to_2D(int n, float *p2d, const float *p3d, const float *camK, int type) {
    using namespace Eigen;
    using namespace define_utils;

    constMapMatXf P3d(p3d, 3, n);
    MapMatXf P2d(p2d, 2, n);
    assert(type<3);

    if(type==0 || type==1){
        VectorXf fxy(2), txy(2);
        if(type==0){
            fxy(0) = camK[0];
            fxy(1) = camK[2];
            txy(0) = camK[1];
            txy(1) = camK[3];
        }else{
            fxy(0) = camK[0];
            fxy(1) = camK[3+1];
            txy(0) = camK[2];
            txy(1) = camK[3+2];
        }
        //P2d = P3d.block(0, 0, 2, n).colwise() * fxy; // error
        //P2d += txy *  P3d.block(2, 0, 1, n); then div z

        //P2d = P2d.rowwise() / P3d.block(2, 0, 1, n) + txy;

        P2d =  P3d.block(0, 0, 2, n).array().rowwise() / P3d.row(2).array();

        P2d.block(0, 0, 1, n) *= fxy(0);
        P2d.block(1, 0, 1, n) *= fxy(1);

        //P2d = P2d.colwise() + txy;
        P2d.colwise() += txy;

        //P2d.block(0, 0, 1, n) = P3d.row(0).array() / P3d.row(2).array() ; // * fxy(0);
        //P2d.block(1, 0, 1, n) = P3d.block(1, 0, 1, n) / P3d.block(2, 0, 1, n) * fxy(1);
        //P2d = P2d.rowwise() / P3d.block(2, 0, 1, n) + txy;


    }else{
        constMapMatXf_RM K(camK, 2, 3);
        //cout<<K<<endl;
        P2d = K * P3d;
        //P2d = P2d.array().rowwise() / P3d.row(2).array();
        //P2d.array().rowwise() /= P3d.row(2).array();
        P2d.array().rowwise() /= P3d.row(2).array();
    }

}