图优化和g2o

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

图优化    

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

    由于误差的存在,zk不可能精确地等于 h(xk),于是就有了误差:ekzk - h(xk)。如果我们以 x为优化变量,以 min Fk(xk) = |ek| 为目标函数,就可以求得 x的最优值。

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

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

u.jpg

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

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

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

1.jpg

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

    图优化的目标函数

2.jpg

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

    图优化的流程:

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

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

3.选择初值,开始迭代;

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

4.求解稀疏线性方程HkΔx = −bk,得到梯度方向;

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

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

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


g2o

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

3.jpg

    上图左边的节点SparseOptimizer类是我们要维护的东西,是一个Optimizable Graph,从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 (继承自 Base Vertex)和很多条(继承自 BaseUnaryEdge, BaseBinaryEdgeBaseMultiEdge)。这些 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讲

上一篇:
下一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于