g2o、Eigen、Mat矩阵类型转换

1. 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 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(i,j);

    return eigenMat;
}

---

Se3->Isometry3D

g2o::Isometry3D toG2oIsometry3D(const cv::Mat &T){
    Eigen::Matrix R;
    R << T.at(0,0), T.at(0,1), T.at(0,2),
            T.at(1,0), T.at(1,1), T.at(1,2),
            T.at(2,0), T.at(2,1), T.at(2,2);
    g2o::Isometry3D ret = (g2o::Isometry3D) Eigen::Quaterniond(R);
    Eigen::Vector3d t(T.at(0,3), T.at(1,3), T.at(2,3));
    ret.translation() = t;
    return ret;
}

g2o::Isometry3D toG2oIsometry3D(const Se3& _se3) {
    return toG2oIsometry3D(_se3.T());
}

---

vector和Eigen::Isometry2f/3f的转换

vector->Isometry2f

inline Eigen::Isometry2f toEigen(float x, float y, float theta)
{
	Eigen::Isometry2f T;
	T.setIdentity();

	float c = cos(theta);
	float s = sin(theta);
	T.linear() << c, -s, s, c;
	T(0, 2) = x;
	T(1, 2) = y;
	return T;
}

 Isometry2f->Isometry3f

inline Eigen::Isometry3d toIsometry3d(const Eigen::Isometry2f& isometry2f) {
	Eigen::Isometry3d isometry3f;
	isometry3f.setIdentity();
	isometry3f.linear().block<2, 2>(0, 0) = isometry2f.linear();
	isometry3f.translation().head<2>() = isometry2f.translation();
	return isometry3f;
}

Isometry3d->Vector

inline Eigen::Isometry3d toEigen(float x, float y, float theta)
{
	Eigen::Isometry2d T;
	T.setIdentity();

	float c = cos(theta);
	float s = sin(theta);
	T.linear() << c, -s, s, c;
	T(0, 2) = x;
	T(1, 2) = y;

	Eigen::Isometry3d isometry3f;
	isometry3f.setIdentity();
	isometry3f.linear().block<2, 2>(0, 0) = T.linear();
	isometry3f.translation().head<2>() = T.translation();
	return isometry3f;
}

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

---

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);

---

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();
}

---

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));

---

参考：

https://blog.csdn.net/hzwwpgmwy/article/details/80712967