【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 -> 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(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 -> 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 把点坐标赋给向量
Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Point3f &cvPoint) //typedef Point3_ cv::Point3f
{
//声明存储转换结果用的变量
Eigen::Matrix<double,3,1> v;
//直接赋值的方法
v << cvPoint.x, cvPoint.y, cvPoint.z;
//返回转换结果
return v;
}
//------------------------------------------------------------------------------
//cv::Mat -> Eigen::MatrixConverter.h
/**
* 提供了一系列的常见转换。
* orb中以cv::Mat为基本存储结构（opencv），到g2o和Eigen需要一个转换。
* 这些转换都很简单，整个文件可以单独从orbslam里抽出来而不影响其他功能.
*/
#ifndef CONVERTER_H
#define CONVERTER_H
#include \n
* 转换后的结果就是吧cv::Mat的每一行直接串联起来。
*
* @param[in] Descriptors 待转换的描述子
* @return std::vector 转换结果
* @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 转换结果
* @note 需要确保输入的数据大小尺寸正确。
*/
static Eigen::Matrix<double,3,1> toVector3d(const cv::Mat &cvVector);
/**
* @brief 将cv::Point3f转换成为Eigen中3x1的矩阵
*
* @param[in] cvPoint 输入的cv表示的三维点坐标
* @return Eigen::Matrix 以Eigen中3x1向量表示的三维点坐标
*/
static Eigen::Matrix<double,3,1> toVector3d(const cv::Point3f &cvPoint);
/**
* @brief 将一个3x3的cv::Mat矩阵转换成为Eigen中的矩阵
*
* @param[in] cvMat3 输入
* @return Eigen::Matrix 转换结果
*/
static Eigen::Matrix<double,3,3> toMatrix3d(const cv::Mat &cvMat3);
/**
* @brief 将给定的cv::Mat类型的旋转矩阵转换成以std::vector类型表示的四元数
*
* @param[in] M 以cv::Mat表示的旋转矩阵
* @return std::vector 四元数
* @note 需要自己保证参数M满足旋转矩阵的定义
* @remark
*/
static std::vector<float> toQuaternion(const cv::Mat &M);
/** @} */
};
}// namespace ORB_SLAM
#endif // CONVERTER_H