SLAM——ORB-SLAM3代码分析（七）Converter
发布时间
阅读量：
阅读量
2021SC@SDUSC
Converter分析
Converter主要是一些常用的转换的实现：比如将g2o类型的数据,cv类型的数据，vector类型的数据进行转换。
- toDescriptorVector 是将描述子转换为描述子向量，其实本质上是cv:Mat->std:vector。先存储转换结果的向量，再创建保留空间，接着在for循环中对于每一个特征点的描述子，从描述子这个矩阵中抽取出来存到向量中，最后返回转换结果。
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;
}- toSE3Quat 是将变换矩阵转换为李代数se3：cv:Mat->g2o::SE3Quat，首先将旋转矩阵提取出来，然后将平移向量提取出来，构造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),
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);
}接下来几个toCvMat 是分别由不同的形式转换成cv::Mat的矩阵形式。
- 第一个是将李代数se3转换为变换矩阵：g2o::SE3Quat->cv::Mat，首先转化成为Eigen中的矩阵形式，然后转换成为cv::Mat的矩阵形式，然后再由Eigen::Matrix->cv::Mat。
- 第二个是将仿射矩阵由g2o::Sim3->cv::Mat，首先将仿射矩阵的旋转部分转换成为Eigen下的矩阵格式，其平移部分也要转换成为Eigen下的矩阵格式，接着获取仿射矩阵的缩放系数，然后构造cv::Mat格式下的仿射矩阵。
- 第三个是由Eigen::Matrix<double,4,4> -> cv::Mat，用于变换矩阵T的中间转换，首先定义存储计算结果的变量，然后利用一个for循环逐个元素赋值，最后返回计算结果
- 第四个和第五个与第三个大同小异，不再赘述。
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::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();
}- toCvSE3 是将给定的旋转矩阵R和平移向量t转换成为 以cv::Mat格式存储的李群SE3（其实就是变换矩阵）：首先生成用于存储转换结果的单位矩阵，用两个for循环分别将旋转矩阵复制到左上角，将旋转矩阵复制到最右侧的一列。
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();
}- toVector3d 是将向量转化为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;
}
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;
}- toMatrix3d 与上者异曲同工，有相似的流程和结构，不再赘述。
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;
}
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;
}- toQuaternion 是将cv::Mat类型的四元数转换成为std::vector型：首先将cv::Mat格式的旋转矩阵转换成为Eigen::Matrix格式，然后利用这个矩阵转换成为四元数格式，最后声明一个这样的向量，将四元数的四个元素分别保存到这个vector中。
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;
}Converter类完成的就是转换的工作，从上述的大部分选取分析中可以基本了解到。Converter类较为简单，接下来会进行对一个很重要的部分——LoopClosing 的分析。
全部评论 (0)
还没有任何评论哟~