主要参考吴博大神的代码
主要关于Opencv 和 g2o Eigen数据格式的转化
#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_SLAM2
{
/**
* @brief 提供了一些常见的转换
*
* orb中以cv::Mat为基本存储结构,到g2o和Eigen需要一个转换
* 这些转换都很简单,整个文件可以单独从orbslam里抽出来而不影响其他功能
*/
class Converter
{
public:
/**
* @brief 一个描述子矩阵到一串单行的描述子向量
*/
static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
/**
* @name toSE3Quat
*/
///@{
/** cv::Mat to g2o::SE3Quat */
static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
/** unimplemented */
static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
///@}
/**
* @name toCvMat
*/
///@{
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 toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t);
///@}
/**
* @name toEigen
*/
///@{
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 std::vector<float> toQuaternion(const cv::Mat &M);
///@}
};
}// namespace ORB_SLAM
#endif // CONVERTER_H
具体实现,代码比较简单,就是数据上转化一下,适合各种库函数直接处理
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Converter.h"
namespace ORB_SLAM2
{
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::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();
}
// 将OpenCVS中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;
}
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;
}
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;
}
} //namespace ORB_SLAM