g2o顶点和边 ICP计算6维度转换矩阵

顶点和边的设置

顶点和边的说明可见上一条博客

顶点，优化变量

/// vertex and edges used in g2o ba 
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
   
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//初始估计值
  virtual void setToOriginImpl() override {
   
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3，更新
  virtual void oplusImpl(const double *update) override {
   
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {
   }

  virtual bool write(ostream &out) const override {
   }
};

设定一元边

误差方程关联的参数为两个三维坐标和一个6维转换矩阵（顶点，优化变量）

/// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> {
   
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//新建边的参数，有一个三维顶点是不变的_points
//另一个三维顶点是_measurement
  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {
   }
  virtual void computeError() override {
   
    const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );
    _error = _measurement - pose->estimate() * _point;
  }

  virtual void linearizeOplus() override {
   
    VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = pose->estimate();
    Eigen::Vector3d xyz_trans = T * _point;
    //新的雅可比矩阵设置方式
    //前3*3为单位阵
    _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
    //后3*3为T*xyz的反对称矩阵
    //Sophus::SO3d::hat() 反对称矩阵
    _jacobianOplusXi.block<3, 3>