图优化和g2o

    我看到高博的好多例子都用到g2o，因此我找了一些博客来学习g2o和图优化的基本知识，这篇博客就是一个简单的总结。

图优化    

    SLAM的核心是根据已有的观测数据，计算机器人的运动轨迹和地图。假设在时刻 k，机器人在位置 x_k 处，用传感器进行了一次观测，得到了数据 z_k，该传感器的观测方程为：z_k = h(x_k)。

    由于误差的存在，z_k不可能精确地等于 h(x_k)，于是就有了误差：e_k = z_k - h(x_k)。如果我们以 x_k 为优化变量，以 min F_k(x_k) = |e_k| 为目标函数，就可以求得 x_k 的最优值。

    SLAM问题的处理方法主要分为滤波和图优化两类，滤波的方法常见的是扩展卡尔曼滤波、粒子滤波、信息滤波等。而图优化，就是把一个常规的优化问题，以图的形式来表述。图(Graph)是由顶点(Vertex)和边(Edge)组成的结构。

    记一个图G={V,E}，其中V为顶点集，E为边集。边可以是有向的或是无向的，对应的图称为有向图或无向图。边可以连接一个顶点(Unary Edge，一元边)、两个顶点(Binary Edge，二元边)或多个顶点(Hyper Edge，多元边)。当一个图中存在连接两个以上顶点的边时，称这个图为超图(Hyper Graph)。在图中，以顶点表示优化变量，以边表示观测方程。一个顶点和多条一元边的示意图如下：

    SLAM问题可以表示成一个超图，其中观测方程有很多形式，下面是几个常用的观测方程和其对应的图优化表示：

    1.机器人两个Pose之间的变换：一条二元边，两个顶点为位姿，边的方程是位姿变换：T1=ΔT*T2。

    2.机器人在某个Pose处用激光测量到了某个空间点，得到了它离自己的距离与角度：二元边，两个顶点分别是一个2D Pose:[x,y,𝜃θ]和一个二维点[λx,λy]，观测数据是距离r和角度b，那么观测方程为：

    3.机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标：二元边，顶点分别为一个3D Pose T和一个空间P=[x,y,z]，观测数据为像素坐标u=[u,v]，则观测方程为：u=K(RP+t)，其中K为相机内参，R,t表示旋转和平移。

    图优化的目标函数：

    e表示误差。x_k可以指一个顶点、两个顶点或多个顶点，取决于边的实际类型。z_k 是测量值。Ω_k 表示该误差的权重矩阵，也就是边的信息矩阵。图优化里每一条边的信息矩阵就是测量协防差矩阵的逆，协防差越小，表示这次测量越准越值得相信，信息权重就越大。

    图优化的流程：

1.选择你想要的图里的节点与边的类型，确定它们的参数化形式；

2.往图里加入实际的节点和边；

3.选择初值，开始迭代；

3.每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；

4.求解稀疏线性方程H_kΔx = −b_k，得到梯度方向；

5.继续用高斯牛顿法(GN)或列文列文伯格-马夸尔特法(LM)进行迭代。

6.如果迭代结束，返回优化值。

    图优化库g2o可以帮我们做好第3-6步。关于GN和LM算法，之前有做过笔记<非线性最小二乘求解方法>。

g2o

    g2o(General Graphic Optimization)，是一个基于图优化的库。之前也有记录过g2o的安装过程<Ceres和g2o的配置和使用>。下图是g2o基本的类结构：

    上图左边的节点SparseOptimizer类是我们要维护的东西，是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 (继承自 Base Vertex)和很多条边(继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。用 SparseOptimizer.addVertex() 和 SparseOptimizer.addEdge() 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize() 完成优化。

    在优化之前，需要指定求解器(Solver)和迭代算法(OptimizationAlgorithm)。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一。同时，这个 Optimization Algorithm 拥有一个Solver，它是一个块求解器(BlockSolver)，它含有两个部分：一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； LinearSolver，一个线性方程的求解器，是用于计算迭代过程中 HΔx = −b，LinearSolver可以从 PCG, CSparse, Choldmod 三者选一。

    作为 g2o 的用户，我们要做的事主要包含以下步骤：

1. 定义顶点和边的类型。

2. 构建图。

3. 选择优化算法。包括选择LinearSolver,BlockSolver,OptimizationAlgorithm。

4. 调用 g2o 进行优化，返回结果。

    有时候我们需要自定义顶点和边，这需要继承g2o中对应的基类。自定义顶点需要覆盖如下方法：

virtual bool read(std::istream& is);  
virtual bool write(std::ostream& os) const;  
virtual void oplusImpl(const number_t* update);  
virtual void setToOriginImpl();

    其中read()，write()函数是读盘和存盘功能，可以不进行覆写，仅仅声明一下就可以。setToOriginImpl()的功能是重置，设定被优化变量的初始值。oplusImpl()的功能是更新，根据增量方程计算出增量之后，通过该函数对估计值进行调整。

    自定义边如要覆盖如下方法：

virtual bool read(std::istream& is);  
virtual bool write(std::ostream& os) const;  
virtual void computeError();  
virtual void linearizeOplus();

    Read()和write()函数同上。computeError()函数是使用当前顶点的值计算的测量值与真实的测量值之间的误差。linearizeOplus()函数是在当前顶点的值下，该误差对优化变量的偏导数，即计算雅可比矩阵。

代码实现

    上篇博文<ICP算法>里，使用高斯牛顿法对ICP求解的结果进行优化，这里改用g2o优化。

    首先定义顶点：

/*
 HyperGraph::Vertex 是个抽象类
 OptimizableGraph::Vertex继承自HyperGraph::Vertex
 而BaseVertex继承自OptimizableGraph：：Vertex
 */

//自定义顶点，继承自BaseVertex类模板，模板参数：顶点的最小维度，顶点的数据类型
//第一个参数D并非是顶点的维度，而是其在流形空间（manifold）的最小表示，这里是李代数的维度
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> 
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  
  //重置优化变量
  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();//_estimate被优化变量
  }
  
  //根据增量方程计算处增量*updata后，该函数对估计值_estimate进行更新，这里估计值通过左乘扰动量(增量)进行更新
  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 {}
};

    然后定义一元边：

/*常用变量的定义：
BaseUnaryEdge定义的变量如下：
    JacobianXiOplusType _jacobianOplusXi;//雅可比矩阵

BaseUnaryEdge继承自BaseEdge，BaseEdge类中定义的变量如下：
    Measurement _measurement;//存储观测值
    InformationType _information;//存储信息矩阵
    ErrorVector _error; //存储计算的误差

BaseEdge继承自OptimizableGraph::Edge，Edge类定义的变量如下：
    int _dimension;

OptimizableGraph::Edge继承自HyperGraph::Edge，里面定义的变量如下： 
    VertexContainer _vertices;//存储边连接的顶点
*/

//自定义边，继承自一元边BaseUnaryEdge模板类，模板参数：观测方程的维度，类型，顶点类型
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> 
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  //构造函数，传入用于计算估计值的点
  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : 
      _point(point) {}

  //使用当前顶点的值计算的测量值与真实的测量值之间的误差。
  virtual void computeError() override {
    //获得当前顶点，顶点由setVertex(int, vertex)设置
    const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );
    //计算观测值与估计值之差，观测值由setMeasurement(type)设置
    _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;
    //计算雅可比
    _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
    _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
  }

  //读存盘
  bool read(istream &in) {}
  bool write(ostream &out) const {}

protected:
  Eigen::Vector3d _point;
};

    接下来，就是定义图，选择优化算法，并进行优化：

void bundleAdjustment(
  const vector<Point3f> &pts1,
  const vector<Point3f> &pts2,
  Mat &R, Mat &t) 
{

  //定义可变尺寸的blocksolver，
  //它的定义：using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
  typedef g2o::BlockSolverX BlockSolverType;
  
  //创建线性求解器类型，这里选择LinearSolverDense
  /*可选择的线性求解器
    LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
    LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
    LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
    LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
    LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
   */
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
  
  
  //创建总的求解器， 梯度下降方法可以从GN, LM, DogLeg 中选，这里选择LM
  /*可选迭代算法，均继承自OptimizationWithHessian，而OptimizationAlgorithmWithHessian继承自OptimizationAlgorithm
    g2o::OptimizationAlgorithmGaussNewton
    g2o::OptimizationAlgorithmLevenberg 
    g2o::OptimizationAlgorithmDogleg 
   */
  auto solver = new g2o::OptimizationAlgorithmLevenberg(
      g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())//创建blockSolver
      );
      
  //稀疏优化器
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出
  
  //添加顶点
  VertexPose *pose = new VertexPose();
  pose->setId(0);
  pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(pose);
  
  //添加边
  for (size_t i = 0; i < pts1.size(); i++) {
    EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
    edge->setVertex(0, pose);//设置顶点
    edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));//设置观测值
    edge->setInformation(Eigen::Matrix3d::Identity());//设置信息矩阵，协方差矩阵之逆
    optimizer.addEdge(edge);//添加边
  }
  
  //初始化参数，
  optimizer.initializeOptimization();
  //进行优化，迭代10次
  optimizer.optimize(10);
  
  //取出优化后的结果
  cout << endl << "after optimization:" << endl;
  cout << "T=\n" << pose->estimate().matrix() << endl;
  // convert to cv::Mat
  Eigen::Matrix3d R_ = pose->estimate().rotationMatrix();
  Eigen::Vector3d t_ = pose->estimate().translation();
  R = (Mat_<double>(3, 3) <<
    R_(0, 0), R_(0, 1), R_(0, 2),
    R_(1, 0), R_(1, 1), R_(1, 2),
    R_(2, 0), R_(2, 1), R_(2, 2)
  );
  t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

参考文献 

[0]半闲居士.深入理解图优化与g2o：图优化篇.https://www.cnblogs.com/gaoxiang12/p/5244828.html .2016-03-07

[1]白巧克力亦唯心.graph slam tutorial : 从推导到应用1.https://blog.csdn.net/heyijia0327/article/details/47686523.2015-10-12

[2]半闲居士.深入理解图优化与g2o：g2o篇.https://www.cnblogs.com/gaoxiang12/p/5304272.html.2016-03-22

[3]Feifanren.g2o学习——g2o整体框架.https://www.cnblogs.com/feifanrensheng/articles/8681952.html.2018-03-31

[4] 计算机视觉life.从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码. https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486858&idx=1&sn=ce458d5eb6b1ad11b065d71899e31a04&chksm=97d7e81da0a0610b1e3e12415b6de1501329920c3074ab5b48e759edbb33d264a73f1a9f9faf&scene=21#wechat_redirect.  2019-01-17

[5]计算机视觉life.从零开始一起学习SLAM | 掌握g2o顶点编程套路. https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486992&idx=1&sn=ecb7c3ef9bd968e51914c2f5b767428d&chksm=97d7eb87a0a062912a9db9fb16a08129f373791fd3918952342d5db46c0bc4880326a7933671&scene=21#wechat_redirect.  2019-02-22

[6]视觉SLAM14讲