g2o、Eigen、Mat矩阵类型转换
点云PCL免费知识星球，点云论文速读。
标题：g2o、Eigen、Mat矩阵类型转换
作者：Leather_Wang
来源：https://me.csdn.net/hzwwpgmwy
排版：particle
本文仅做学术分享，已获得作者授权转载，未经允许请勿二次转载！欢迎各位加入免费知识星球，获取PDF文档，欢迎转发朋友圈，分享快乐。
希望有更多的小伙伴能够加入我们，一起开启论文阅读，相互分享的微信群。参与和分享的方式：dianyunpcl@163.com
Eigen矩阵赋值
1） 使用row或者col
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
R.row(0) = Eigen::Vector3d(-0.0134899,-0.997066,0.0753502);
R.row(1) = Eigen::Vector3d(-0.0781018,-0.0740761,-0.99419);
R.row(2) = Eigen::Vector3d(0.996854,-0.0192965,-0.0768734);2）使用block
注意：g2o中使用的g2o::Matrix3D是typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> Matrix3D;，即还是使用的Eigen矩阵
g2o::Matrix3D R = g2o::Matrix3D::Identity();
R.block<2,2>(0,0) << cos(theta),-sin(theta),sin(theta),cos(theta);Se3<->SE3Quat
Matrix3D Rbc=toEigenMatrix3d(se3bc.R());
Vector3D tbc=toG2oVector3D(se3bc.tvec);
g2o::SE3Quat Tbc=g2o::SE3Quat(Rbc, tbc);Se2->SE3Quat
方法一：
Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure.x, measure_se2.y, 0));方法二：
g2o::SE3Quat toG2oSE3Quat(const g2o::SE2& se2)
{
g2o::Vector3D v2 = se2.toVector();
double x = v2(0);
double y = v2(1);
double theta = v2(2);
g2o::Matrix3D R = g2o::Matrix3D::Identity();
R.block<2,2>(0,0) << cos(theta),-sin(theta),sin(theta),cos(theta);
g2o::Vector3D t;
t << x,y,0;
return g2o::SE3Quat(R,t);
}Mat->Eigen
Eigen::MatrixXd toEigenMatrixXd(const cv::Mat &cvMat)
{
Eigen::MatrixXd eigenMat;
eigenMat.resize(cvMat.rows, cvMat.cols);
for (int i=0; i<cvMat.rows; i++)
for (int j=0; j<cvMat.cols; j++)
eigenMat(i,j) = cvMat.at<float>(i,j);
return eigenMat;
}Se3->Isometry3D
g2o::Isometry3D toG2oIsometry3D(const cv::Mat &T){
Eigen::Matrix<double,3,3> R;
R << T.at<float>(0,0), T.at<float>(0,1), T.at<float>(0,2),
T.at<float>(1,0), T.at<float>(1,1), T.at<float>(1,2),
T.at<float>(2,0), T.at<float>(2,1), T.at<float>(2,2);
g2o::Isometry3D ret = (g2o::Isometry3D) Eigen::Quaterniond(R);
Eigen::Vector3d t(T.at<float>(0,3), T.at<float>(1,3), T.at<float>(2,3));
ret.translation() = t;
return ret;
}
g2o::Isometry3D toG2oIsometry3D(const Se3& _se3) {
return toG2oIsometry3D(_se3.T());
}Isometry3D与SE3Quat互相转换
1）SE3Quat 类型
SE3Quat 是g2o中老版本相机位姿的表示，内部使用四元数+平移向量存储位姿，同时支持李代数上的运算，例如对数映射（log函数）、李代数上增量（update函数）、指数映射（exp函数）、伴随矩阵（adj函数）等操作
g2o中定义顶点VertexSE3Expmap中的oplusImpl函数实现就是使用左乘更新
class VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
virtual void oplusImpl(const double* update_) {
Eigen::Map<const Vector6d> update(update_);
// Vector6d update;
// update[1] = update_[1]; update[3] = update_[3]; update[5] = update_[5];
setEstimate(SE3Quat::exp(update)*estimate());
}
......使用SE3Quat成员函数to_homogeneous_matrix()将SE3Quat->Eigen::Matrix<double,4,4>
2）Isometry3D->SE3Quat
/**
* convert an Isometry3D to the old SE3Quat class
*/
G2O_TYPES_SLAM3D_API SE3Quat toSE3Quat(const Isometry3D& t);3）SE3Quat->Isometry3D
/**
* convert from an old SE3Quat into Isometry3D
*/
G2O_TYPES_SLAM3D_API Isometry3D fromSE3Quat(const SE3Quat& t);4）Mat->Se3
注意对Mat操作使用_T.colRange(0,3).rowRange(0,3).clone()获取矩阵块
Se3::Se3(const cv::Mat &_T)
{
assert(_T.type() == CV_32FC1);
Mat R = _T.colRange(0,3).rowRange(0,3).clone();
Rodrigues(R, rvec);
tvec = _T.col(3).rowRange(0,3).clone();
}5）Mat类型操作 copyTo和colRange使用
Mat Rcd = Mat::zeros(3,3,CV_32FC1);
rx.copyTo(Rcd.colRange(0,1));
ry.copyTo(Rcd.colRange(1,2));
rz.copyTo(Rcd.colRange(2,3));