opencv的Mat与Eigen的Matrix相互转换

素颜马尾好姑娘i 2022-12-15 03:56 486阅读 0赞

**目录**

1 opencv的Mat与Eigen的Matrix相互转换

1.1 Eigen 转 cv::Mat

1.2 cv::Mat 转 Eigen

1.3 float 与 double 互转

2 SE(3)：把cv的旋转矩阵转换为Eigen中旋转矩阵的赋值

3 ORB-SLAM3 各种类型矩阵变换函数：

--------------------

# 1 opencv的Mat与Eigen的Matrix相互转换 #

首先包含这几个头文件，据说Eigen相关的要放在前面。

#include <Eigen/Core>
    #include <Eigen/Dense>
    #include <opencv2/core/eigen.hpp>
    #include <opencv2/opencv.hpp>

## 1.1 Eigen 转 cv::Mat ##

关键函数：cv::eigen2cv

Eigen::Matrix<float, 2, 3> matrix_23f;
    matrix_23f << 1, 2, 3, 
    			  4, 5, 6;
    cv::Mat mat_23f;
    cv::eigen2cv(matrix_23f, mat_23f);

## 1.2 cv::Mat 转 Eigen ##

关键函数：cv::cv2eigen

cv::Mat mat_23f(2, 3, CV_32F); // 也可以不加括号后面的，直接定义 cv::Mat mat_23f;
    mat_23f = (cv::Mat_<float>(2, 3) << 
    	1, 2, 3, 
    	4, 5, 6);
    Eigen::Matrix<float, 2, 3> matrix_23f;
    cv::cv2eigen(mat_23f, matrix_23f);

## 1.3 float 与 double 互转 ##

在Eigen和opencv中，矩阵的精度是不能隐式转换的，必须**显式转换**。  
**eigen：**  
关键函数：matrix\_23f.cast<double>()

Eigen::Matrix<float, 2, 3> matrix_23f;
    matrix_23f << 1, 2, 3, 
    			  4, 5, 6;
    Eigen::Matrix<double, 2, 3> matrix_23d = matrix_23f.cast<double>();

**opencv：**  
关键函数：mat\_23f.convertTo(mat\_23d, CV\_64F)

cv::Mat mat_23f(2, 3, CV_32F);
    mat_23f = (cv::Mat_<float>(2, 3) << 
    	1, 2, 3, 
    	4, 5, 6);
    cv::Mat mat_23d(2, 3, CV_64F);
    mat_23f.convertTo(mat_23d, CV_64F);

# 2 SE(3)：把cv的旋转矩阵转换为Eigen中旋转矩阵的赋值 #

今天处理ORBslam的位姿进行点云拼接时碰到了问题  
ORBslam获得的位姿旋转矩阵是一个在cv::Mat下定义的4×4的矩阵，需要转换到Eigen下，因为想要使用相应的点云转换函数  
因此使用下面的函数进行转换

//把mat类型的旋转矩阵SE(3)转换为Eigen下的SE(3)
    Eigen::Isometry3d Mat2Isometry3d(cv::Mat R)
    {
    	Eigen::Isometry3d T;
    	{	
    		cout<<"Convert Failed!"<<endl;
    		return Eigen::Isometry3d::Identity();
    	}
    	for(int i=0;i<4;i++)
    		for(int j=0;j<4;j++)
    		{
    			T(i,j)=R.at<float>(i,j);	
    		}
    	return T;
    }

需要注意的是在这一行`T(i,j)=R.at<float>(i,j);`

**如果把float换成double，就会出现T的值非常的乱，与矩阵R没有任何关系，因此这儿的格式一定要注意**

# **3 ORB-SLAM3 各种类型矩阵变换函数：** #

**Converter.h**

#ifndef CONVERTER_H
    #define CONVERTER_H
    
    #include<opencv2/core/core.hpp>
    
    #include<Eigen/Dense>
    #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
    #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
    
    namespace ORB_SLAM3
    {
    
    class Converter
    {
    public:
        static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
    
        static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
        static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
    
        static cv::Mat toCvMat(const g2o::SE3Quat &SE3);
        static cv::Mat toCvMat(const g2o::Sim3 &Sim3);
        static cv::Mat toCvMat(const Eigen::Matrix<double,4,4> &m);
        static cv::Mat toCvMat(const Eigen::Matrix3d &m);
        static cv::Mat toCvMat(const Eigen::Matrix<double,3,1> &m);
        static cv::Mat toCvMat(const Eigen::MatrixXd &m);
    
        static cv::Mat toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t);
        static cv::Mat tocvSkewMatrix(const cv::Mat &v);
    
        static Eigen::Matrix<double,3,1> toVector3d(const cv::Mat &cvVector);
        static Eigen::Matrix<double,3,1> toVector3d(const cv::Point3f &cvPoint);
        static Eigen::Matrix<double,3,3> toMatrix3d(const cv::Mat &cvMat3);
        static Eigen::Matrix<double,4,4> toMatrix4d(const cv::Mat &cvMat4);
    
        //john update
        static Eigen::Matrix<float,3,3> toMatrix3f(const cv::Mat &cvMat3);
        static Eigen::Matrix<float,4,4> toMatrix4f(const cv::Mat &cvMat4);
    
        static std::vector<float> toQuaternion(const cv::Mat &M);
    
        static bool isRotationMatrix(const cv::Mat &R);
        static std::vector<float> toEuler(const cv::Mat &R);
    
    };
    
    }// namespace ORB_SLAM
    
    #endif // CONVERTER_H

**Converter.cpp**

#include "Converter.h"
    
    namespace ORB_SLAM3
    {
    
    std::vector<cv::Mat> Converter::toDescriptorVector(const cv::Mat &Descriptors)
    {
        std::vector<cv::Mat> vDesc;
        vDesc.reserve(Descriptors.rows);
        for (int j=0;j<Descriptors.rows;j++)
            vDesc.push_back(Descriptors.row(j));
    
        return vDesc;
    }
    
    g2o::SE3Quat Converter::toSE3Quat(const cv::Mat &cvT)
    {
        Eigen::Matrix<double,3,3> R;
        R << cvT.at<float>(0,0), cvT.at<float>(0,1), cvT.at<float>(0,2),
             cvT.at<float>(1,0), cvT.at<float>(1,1), cvT.at<float>(1,2),
             cvT.at<float>(2,0), cvT.at<float>(2,1), cvT.at<float>(2,2);
    
        Eigen::Matrix<double,3,1> t(cvT.at<float>(0,3), cvT.at<float>(1,3), cvT.at<float>(2,3));
    
        return g2o::SE3Quat(R,t);
    }
    
    cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3)
    {
        Eigen::Matrix<double,4,4> eigMat = SE3.to_homogeneous_matrix();
        return toCvMat(eigMat);
    }
    
    cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3)
    {
        Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix();
        Eigen::Vector3d eigt = Sim3.translation();
        double s = Sim3.scale();
        return toCvSE3(s*eigR,eigt);
    }
    
    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<float>(i,j)=m(i,j);
    
        return cvMat.clone();
    }
    
    cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m)
    {
        cv::Mat cvMat(3,3,CV_32F);
        for(int i=0;i<3;i++)
            for(int j=0; j<3; j++)
                cvMat.at<float>(i,j)=m(i,j);
    
        return cvMat.clone();
    }
    
    cv::Mat Converter::toCvMat(const Eigen::MatrixXd &m)
    {
        cv::Mat cvMat(m.rows(),m.cols(),CV_32F);
        for(int i=0;i<m.rows();i++)
            for(int j=0; j<m.cols(); j++)
                cvMat.at<float>(i,j)=m(i,j);
    
        return cvMat.clone();
    }
    
    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<float>(i)=m(i);
    
        return cvMat.clone();
    }
    
    cv::Mat Converter::toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t)
    {
        cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F);
        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {
                cvMat.at<float>(i,j)=R(i,j);
            }
        }
        for(int i=0;i<3;i++)
        {
            cvMat.at<float>(i,3)=t(i);
        }
    
        return cvMat.clone();
    }
    
    Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Mat &cvVector)
    {
        Eigen::Matrix<double,3,1> v;
        v << cvVector.at<float>(0), cvVector.at<float>(1), cvVector.at<float>(2);
    
        return v;
    }
    
    Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Point3f &cvPoint)
    {
        Eigen::Matrix<double,3,1> v;
        v << cvPoint.x, cvPoint.y, cvPoint.z;
    
        return v;
    }
    
    Eigen::Matrix<double,3,3> Converter::toMatrix3d(const cv::Mat &cvMat3)
    {
        Eigen::Matrix<double,3,3> M;
    
        M << cvMat3.at<float>(0,0), cvMat3.at<float>(0,1), cvMat3.at<float>(0,2),
             cvMat3.at<float>(1,0), cvMat3.at<float>(1,1), cvMat3.at<float>(1,2),
             cvMat3.at<float>(2,0), cvMat3.at<float>(2,1), cvMat3.at<float>(2,2);
    
        return M;
    }
    
    //john update 2021-03-31
    Eigen::Matrix<float,3,3> Converter::toMatrix3f(const cv::Mat &cvMat3)
    {
        Eigen::Matrix<float,3,3> M;
    
        M << cvMat3.at<float>(0,0), cvMat3.at<float>(0,1), cvMat3.at<float>(0,2),
                cvMat3.at<float>(1,0), cvMat3.at<float>(1,1), cvMat3.at<float>(1,2),
                cvMat3.at<float>(2,0), cvMat3.at<float>(2,1), cvMat3.at<float>(2,2);
    
        return M;
    }
    
    Eigen::Matrix<double,4,4> Converter::toMatrix4d(const cv::Mat &cvMat4)
    {
        Eigen::Matrix<double,4,4> M;
    
        M << cvMat4.at<float>(0,0), cvMat4.at<float>(0,1), cvMat4.at<float>(0,2), cvMat4.at<float>(0,3),
             cvMat4.at<float>(1,0), cvMat4.at<float>(1,1), cvMat4.at<float>(1,2), cvMat4.at<float>(1,3),
             cvMat4.at<float>(2,0), cvMat4.at<float>(2,1), cvMat4.at<float>(2,2), cvMat4.at<float>(2,3),
             cvMat4.at<float>(3,0), cvMat4.at<float>(3,1), cvMat4.at<float>(3,2), cvMat4.at<float>(3,3);
        return M;
    }
    
    //john update 2021-03-31
    Eigen::Matrix<float,4,4> Converter::toMatrix4f(const cv::Mat &cvMat4)
    {
        Eigen::Matrix<float,4,4> M;
    
        M << cvMat4.at<float>(0,0), cvMat4.at<float>(0,1), cvMat4.at<float>(0,2), cvMat4.at<float>(0,3),
                cvMat4.at<float>(1,0), cvMat4.at<float>(1,1), cvMat4.at<float>(1,2), cvMat4.at<float>(1,3),
                cvMat4.at<float>(2,0), cvMat4.at<float>(2,1), cvMat4.at<float>(2,2), cvMat4.at<float>(2,3),
                cvMat4.at<float>(3,0), cvMat4.at<float>(3,1), cvMat4.at<float>(3,2), cvMat4.at<float>(3,3);
        return M;
    }
    
    std::vector<float> Converter::toQuaternion(const cv::Mat &M)
    {
        Eigen::Matrix<double,3,3> eigMat = toMatrix3d(M);
        Eigen::Quaterniond q(eigMat);
    
        std::vector<float> v(4);
        v[0] = q.x();
        v[1] = q.y();
        v[2] = q.z();
        v[3] = q.w();
    
        return v;
    }
    
    cv::Mat Converter::tocvSkewMatrix(const cv::Mat &v)
    {
        return (cv::Mat_<float>(3,3) <<             0, -v.at<float>(2), v.at<float>(1),
                v.at<float>(2),               0,-v.at<float>(0),
                -v.at<float>(1),  v.at<float>(0),              0);
    }
    
    bool Converter::isRotationMatrix(const cv::Mat &R)
    {
        cv::Mat Rt;
        cv::transpose(R, Rt);
        cv::Mat shouldBeIdentity = Rt * R;
        cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());
    
        return  cv::norm(I, shouldBeIdentity) < 1e-6;
    
    }
    
    std::vector<float> Converter::toEuler(const cv::Mat &R)
    {
        assert(isRotationMatrix(R));
        float sy = sqrt(R.at<float>(0,0) * R.at<float>(0,0) +  R.at<float>(1,0) * R.at<float>(1,0) );
    
        bool singular = sy < 1e-6; // If
    
        float x, y, z;
        if (!singular)
        {
            x = atan2(R.at<float>(2,1) , R.at<float>(2,2));
            y = atan2(-R.at<float>(2,0), sy);
            z = atan2(R.at<float>(1,0), R.at<float>(0,0));
        }
        else
        {
            x = atan2(-R.at<float>(1,2), R.at<float>(1,1));
            y = atan2(-R.at<float>(2,0), sy);
            z = 0;
        }
    
        std::vector<float> v_euler(3);
        v_euler[0] = x;
        v_euler[1] = y;
        v_euler[2] = z;
    
        return v_euler;
    }
    
    } //namespace ORB_SLAM

参考：  
[https://blog.csdn.net/weixin\_42587961/article/details/94446580][https_blog.csdn.net_weixin_42587961_article_details_94446580]

[https://blog.csdn.net/jqw11/article/details/97777524][https_blog.csdn.net_jqw11_article_details_97777524]

[https://humfrey.blog.csdn.net/article/details/107387479][https_humfrey.blog.csdn.net_article_details_107387479]

[https://blog.csdn.net/qq\_34122731/article/details/91467938][https_blog.csdn.net_qq_34122731_article_details_91467938]

[https_blog.csdn.net_weixin_42587961_article_details_94446580]: https://blog.csdn.net/weixin_42587961/article/details/94446580
[https_blog.csdn.net_jqw11_article_details_97777524]: https://blog.csdn.net/jqw11/article/details/97777524
[https_humfrey.blog.csdn.net_article_details_107387479]: https://humfrey.blog.csdn.net/article/details/107387479
[https_blog.csdn.net_qq_34122731_article_details_91467938]: https://blog.csdn.net/qq_34122731/article/details/91467938