表示位姿的数据结构相互转换。

1.T->T

opencv ->eigen

Eigen::Matrix4d opencv_T_to_matrix4d(const cv::Mat &cvT)
{
    Eigen::Matrix4d e_T;
    e_T << cvT.at<double>(0,0), cvT.at<double>(0,1), cvT.at<double>(0,2),cvT.at<double>(0,3),
         cvT.at<double>(1,0), cvT.at<double>(1,1), cvT.at<double>(1,2),cvT.at<double>(1,3),
         cvT.at<double>(2,0), cvT.at<double>(2,1), cvT.at<double>(2,2),cvT.at<double>(2,3),
         cvT.at<double>(3,0), cvT.at<double>(3,1), cvT.at<double>(3,2),cvT.at<double>(3,3);
    return e_T;
}

eigen->opencv

cv::Mat Converter::toCvMat(const Eigen::Matrix<double,4,4> &m)
{
    cv::Mat cvMat(4,4,CV_32F);
    for(int i=0;i<4;i++)
        for(int j=0; j<4; j++)
            cvMat.at<double>(i,j)=m(i,j);

    return cvMat.clone();
}

2.R->R

opencv ->eigen

Eigen::Matrix3d toMatrix3d(const cv::Mat &cvMat3)
{
    Eigen::Matrix3d M;

    M << cvMat3.at<double>(0,0), cvMat3.at<double>(0,1), cvMat3.at<double>(0,2),
         cvMat3.at<double>(1,0), cvMat3.at<double>(1,1), cvMat3.at<double>(1,2),
         cvMat3.at<double>(2,0), cvMat3.at<double>(2,1), cvMat3.at<double>(2,2);

    return M;
}

eigen->opencv

cv::Mat Converter::toCvMat(const Eigen::Matrix<double,3,1> &m)
{
    cv::Mat cvMat(3,1,CV_32F);
    for(int i=0;i<3;i++)
            cvMat.at<double>(i)=m(i);

    return cvMat.clone();
}

3.T->(q,t)

opencv ->eigen

void opencv_T_to_q_t(const cv::Mat &cvT,Eigen::Quaterniond& q, Eigen::Vector3d& t)
{
    Eigen::Matrix3d e_R;
    e_R << cvT.at<double>(0,0), cvT.at<double>(0,1), cvT.at<double>(0,2),
         cvT.at<double>(1,0), cvT.at<double>(1,1), cvT.at<double>(1,2),
         cvT.at<double>(2,0), cvT.at<double>(2,1), cvT.at<double>(2,2);
    
    q=Eigen::Quaterniond(e_R);
    t.x()=cvT.at<double>(0,3);
    t.y()=cvT.at<double>(1,3);
    t.z()=cvT.at<double>(2,3);
}