opencv转eigen数据结构笔记

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

1.T->T

opencv ->eigen

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

eigen->opencv

cv::Mat Converter::toCvMat(const Eigen::Matrix &m)
{
    cv::Mat cvMat(4,4,CV_32F);
    for(int i=0;i<4;i++)
        for(int j=0; j<4; j++)
            cvMat.at(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(0,0), cvMat3.at(0,1), cvMat3.at(0,2),
         cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2),
         cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(2,2);

    return M;
}

eigen->opencv

cv::Mat Converter::toCvMat(const Eigen::Matrix &m)
{
    cv::Mat cvMat(3,1,CV_32F);
    for(int i=0;i<3;i++)
            cvMat.at(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(0,0), cvT.at(0,1), cvT.at(0,2),
         cvT.at(1,0), cvT.at(1,1), cvT.at(1,2),
         cvT.at(2,0), cvT.at(2,1), cvT.at(2,2);
    
    q=Eigen::Quaterniond(e_R);
    t.x()=cvT.at(0,3);
    t.y()=cvT.at(1,3);
    t.z()=cvT.at(2,3);
}

4.Eigen R,t->T T->R,t

void Rt2T(Eigen::Matrix3d R,Eigen::Vector3d t,Eigen::Matrix4d& T)
{

    T<

注意，这段代码有问题，实际测试的时候可能会有问题，要用输入const引用方式，如下

static void Rt2T(const Eigen::Matrix3d& R,const Eigen::Vector3d& t,Eigen::Matrix4d& T)
{
    T<

矩阵求逆

  Utility::Rt2T(last_R,last_P,T_b);
  cout<<"before imu T_b"<