【orbslam2源码解析】 【include/Converter.h】【src/Converter.cc】

Converter.cc

    /*
     *  ORB-SLAM2中一些常用的转换的实现
     */


    #include "Converter.h"
    namespace ORB_SLAM2//命名空间
    {

    //------------------------------------------------------------------------------
    std::vector<cv::Mat> Converter::toDescriptorVector(const cv::Mat &Descriptors)//toDescriptorVector将描述子转换为描述子向量，其实本质上是cv:Mat->std:vector
    {
        std::vector<cv::Mat> vDesc;    //存储转换结果的向量
        //创建保留空间
        vDesc.reserve(Descriptors.rows); //vector 下的reserve函数 储存vector下的rows 行
        //对于每一个特征点的描述子
        for (int j=0;j<Descriptors.rows;j++)
            //从描述子这个矩阵中抽取出来存到向量中
            vDesc.push_back(Descriptors.row(j)); 

        //返回转换结果
        return vDesc; //返回描述子向量结果
    }

    //------------------------------------------------------------------------------
    //将变换矩阵转换为李代数se3：cv:Mat->g2o::SE3Quat
    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), //Returns a reference to the specified array element. 返回对指定数组元素的引用 
             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)); //平移向量是3,1 矩阵

        //构造g2o::SE3Quat类型并返回
        return g2o::SE3Quat(R,t);//构造g2o类型需要拆分T 为R，t
    }
    //------------------------------------------------------------------------------
    cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3)//重载 1传入李代数se3（ipsino）转换为变换矩阵（T）：g2o::SE3Quat->cv::Mat  
    {
        Eigen::Matrix<double,4,4> eigMat = SE3.to_homogeneous_matrix();///在实际操作上，首先转化成为Eigen中的矩阵形式，然后转换成为cv::Mat的矩阵形式。把se3对应的李群传给eigMat
        return toCvMat(eigMat);    //然后再由Eigen::Matrix->cv::Mat
    }



    //------------------------------------------------------------------------------
    //将仿射矩阵由g2o::Sim3->cv::Mat
    cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3) //重载 2传入仿射矩阵
    {
        Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix();///首先将仿射矩阵的旋转部分转换成为Eigen下的矩阵格式
        Eigen::Vector3d eigt = Sim3.translation();///对于仿射矩阵的平移部分也是要转换成为Eigen下的矩阵格式

        double s = Sim3.scale();///获取仿射矩阵的缩放系数
        ///然后构造cv::Mat格式下的仿射矩阵
        ///感觉这里的sim3就是在se3的基础上多了一个缩放系数，但是实际上真的是这样吗？
        return toCvSE3(s*eigR,eigt);
    }
    //------------------------------------------------------------------------------
    //Eigen::Matrix<double,4,4> -> cv::Mat，用于变换矩阵T的中间转换
    cv::Mat Converter::toCvMat(const Eigen::Matrix<double,4,4> &m)
    {
        cv::Mat cvMat(4,4,CV_32F);//首先定义存储计算结果的变量叫做cvMat,这个计算结果是4,4
    //    Array type. Use CV_8UC1, ..., CV_64FC4 to create 1-4 channel matrices, or CV_8UC(n), ..., CV_64FC(n) to create multi-channel (up to CV_CN_MAX channels) matrices. 
        //然后逐个元素赋值
        for(int i=0;i<4;i++)
            for(int j=0; j<4; j++)
                cvMat.at<float>(i,j)=m(i,j); //at<float>(i,j)是对m(i,j)的引用

        //返回计算结果，还是用深拷贝函数
        return cvMat.clone();//clone（）函数用原型实例指定创建对象的种类，并且通过拷贝这些原型创建新的对象
    }
    //------------------------------------------------------------------------------
    //Eigen::Matrix3d -> cv::Mat 
    cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m)//这里是旋转矩阵到cv::Mat的过程
    {
        //首先定义存储计算结果的变量
        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();
    }
    //------------------------------------------------------------------------------
    //Eigen::Matrix<double,3,1> -> cv::Mat//3,1向量到cv::Mat的过程
    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();
    }
    //------------------------------------------------------------------------------
    //将给定的旋转矩阵R和平移向量t转换成为 以cv::Mat格式存储的李群SE3（T）
    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();    //返回计算结果
    }
    //------------------------------------------------------------------------------
    // 将OpenCV中Mat类型的向量转化为Eigen中Matrix类型的变量
    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;    //返回转换结果
    }
    //------------------------------------------------------------------------------
    //cv::Point3f -> Eigen::Matrix<double,3,1> 把点坐标赋给向量
    Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Point3f &cvPoint) //typedef Point3_<float> cv::Point3f
    {
        //声明存储转换结果用的变量
        Eigen::Matrix<double,3,1> v;
        //直接赋值的方法
        v << cvPoint.x, cvPoint.y, cvPoint.z;
        //返回转换结果
        return v;
    }
    //------------------------------------------------------------------------------
    //cv::Mat -> Eigen::Matrix<double,3,3>
    Eigen::Matrix<double,3,3>  Converter::toMatrix3d(const cv::Mat &cvMat3) //传入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;
    }
    //------------------------------------------------------------------------------
    //将cv::Mat类型的四元数转换成为std::vector型
    std::vector<float> Converter::toQuaternion(const cv::Mat &M)
    {
        //首先将cv::Mat格式的旋转矩阵转换成为Eigen::Matrix格式
        Eigen::Matrix<double,3,3> eigMat = toMatrix3d(M);
        //然后利用这个矩阵转换成为四元数格式
        Eigen::Quaterniond q(eigMat); 
        //最后声明一个这样的向量
        std::vector<float> v(4);
        //将四元数的四个元素分别保存到这个vector中
        ///@note 注意,使用std::vector存储的四元数的顺序是x y z w
        v[0] = q.x();
        v[1] = q.y();
        v[2] = q.z();
        v[3] = q.w();
        //返回转换结果
        return v;
    }

    } //namespace ORB_SLAM

Converter.h

    /**

     *  提供了一系列的常见转换。
     * orb中以cv::Mat为基本存储结构（opencv），到g2o和Eigen需要一个转换。
     * 这些转换都很简单，整个文件可以单独从orbslam里抽出来而不影响其他功能.
     */

    #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"

    /**
     * @brief ORB-SLAM2 自定义的命名空间。 
     * @details 该命名空间中包含了所有的ORB-SLAM2的组件。
     * 
     */
    namespace ORB_SLAM2
    {

    /**
     * @brief 实现了 ORB-SLAM2中的一些常用的转换。 
     * @details 注意这是一个完全的静态类，没有成员变量，所有的成员函数均为静态的。
     */
    class Converter//注意这是一个完全的静态类，没有成员变量，所有的成员函数均为静态的。
    {
    public:

        /**
         * @brief 描述子矩阵到单行的描述子向量的转换. 
         * @details cv::Mat -> std::vector<cv::Mat> \n
         * 转换后的结果就是吧cv::Mat的每一行直接串联起来。
         * 
         * @param[in] Descriptors 待转换的描述子
         * @return std::vector<cv::Mat> 转换结果
         * @note  应当注意，这里的描述子矩阵是多个单行的cv::Mat组成的。
         */
        static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);//静态成员函数

        /**
         * @name toSE3Quat
         * @details 将不同格式存储的位姿统一转换成为g2o::SE3Quat格式存储
         * @{
         */

        /**
         * @brief 将以cv::Mat格式存储的位姿转换成为g2o::SE3Quat类型
         * 
         * @param[in] 以cv::Mat格式存储的位姿
         * @return g2o::SE3Quat 将以g2o::SE3Quat格式存储的位姿
         */

        static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
        /**
         * @brief 将以g2o::Sim3格式存储的位姿转换成为g2o::SE3Quat类型
         * @param[in] gSim3 以g2o::Sim3格式存储的位姿
         * @return g2o::SE3Quat 
         */
        static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);

        /** @} */

        /**
         * @name toCvMat \n
         * @details 将各种格式转换成为cv::Mat存储
         * @{
         */

        /**
         * @brief 将以g2o::SE3Quat格式存储的位姿转换成为cv::Mat格式
         * 
         * @param[in] SE3 输入的g2o::SE3Quat格式存储的、待转换的位姿
         * @return cv::Mat 转换结果
         * @remark 
         */
        static cv::Mat toCvMat(const g2o::SE3Quat &SE3);
        /**
         * @brief 将以g2o::Sim3格式存储的位姿转换成为cv::Mat格式
         * 
         * @param[in] Sim3 输入的g2o::Sim3格式存储的、待转换的位姿
         * @return cv::Mat 转换结果
         * @remark
         */
        static cv::Mat toCvMat(const g2o::Sim3 &Sim3);
        /**
         * @brief 将4x4 double型Eigen矩阵存储的位姿转换成为cv::Mat格式
         * 
         * @paramp[in] m 输入Eigen矩阵
         * @return cv::Mat 转换结果
         * @remark
         */
        static cv::Mat toCvMat(const Eigen::Matrix<double,4,4> &m);
        /**
         * @brief 将一个3x1的Eigen行向量转换成为cv::Mat格式
         * 
         * @param[in] m 3x1的Eigen行向量
         * @return cv::Mat 转换结果
         */
        static cv::Mat toCvMat(const Eigen::Matrix3d &m);
        /**
         * @brief 将一个3x1的Eigen行向量转换成为cv::Mat格式
         * 
         * @param[in] m 3x1的Eigen行向量
         * @return cv::Mat 转换结果
         */
        static cv::Mat toCvMat(const Eigen::Matrix<double,3,1> &m);
        /**
         * @brief 将给定的旋转矩阵和平移向量转换为以cv::Mat存储的李群SE3
         * @details 其实就是组合旋转矩阵和平移向量来构造SE3
         * @param[in] R 旋转矩阵
         * @param[in] t 平移向量
         * @return cv::Mat 李群SE3
         */
        static cv::Mat toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t);
        /** @} */


        /**
         * @name toEigen
         * @details 将给出的数据以Eigen库中对应的数据类型存储
         * @{
         */

        /**
         * @brief 将cv::Mat类型数据转换成为3x1的Eigen矩阵
         * 
         * @param[in] cvVector 待转换的数据
         * @return Eigen::Matrix<double,3,1> 转换结果
         * @note 需要确保输入的数据大小尺寸正确。
         */
        static Eigen::Matrix<double,3,1> toVector3d(const cv::Mat &cvVector);
        /**
         * @brief 将cv::Point3f转换成为Eigen中3x1的矩阵
         * 
         * @param[in] cvPoint 输入的cv表示的三维点坐标
         * @return Eigen::Matrix<double,3,1> 以Eigen中3x1向量表示的三维点坐标
         */
        static Eigen::Matrix<double,3,1> toVector3d(const cv::Point3f &cvPoint);
        /**
         * @brief 将一个3x3的cv::Mat矩阵转换成为Eigen中的矩阵
         * 
         * @param[in] cvMat3 输入
         * @return Eigen::Matrix<double,3,3> 转换结果
         */
        static Eigen::Matrix<double,3,3> toMatrix3d(const cv::Mat &cvMat3);
        /**
         * @brief 将给定的cv::Mat类型的旋转矩阵转换成以std::vector<float>类型表示的四元数
         * 
         * @param[in] M 以cv::Mat表示的旋转矩阵
         * @return std::vector<float> 四元数
         * @note 需要自己保证参数M满足旋转矩阵的定义
         * @remark
         */
        static std::vector<float> toQuaternion(const cv::Mat &M);

        /** @} */
    };

    }// namespace ORB_SLAM

    #endif // CONVERTER_H