g2o 使用

g2o

SLAM 中的图优化库。图中的节点是待优化的变量，边是误差。
$$F(\vec{x})=\sum _{k\in C}\overrightarrow{e_k}(\overrightarrow{x_k},\overrightarrow{z_k}{)}^T{\Omega }_k\overrightarrow{e_k}(\overrightarrow{x_k},\overrightarrow{z_k})$$

${\Omega }_k$ 成为信息矩阵。要在 Edge 中设置。

数据结构

既然是图，就有图的三个结构。OptimizableGraph表示优化的图。OptimizableGraph::Vertex 和 OptimizableGraph::Edge 表示图中的定点和边。库中定义了一些模板类用来扩展

1. BaseVertex：节点的模板。需要实现oplusImpl（用导数更新变量，不一定是直接相加）和 setToOriginImpl （初始化函数）。以一个三维点作为变量为例。

 class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexSBAPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    virtual void setToOriginImpl() {
      _estimate.fill(0.); // 初始 X,Y,Z 全部为0
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Map<const Vector3d> v(update);
      _estimate += v;
    }
};

Eigen::Map<const Vector3d> v(update); 是将数组的指针传给 Eigen，转化为 Vector3d 类型。

一些常用方法说明：

• .setEstimate() 赋初值。
• .setId() 赋索引值。方便之后在图中查找，与optimizer.vertex(id)配合。
• .setFixed() 设为常量。
• .estimate() 返回状态变量的值，优化完后读取。
• .setMarginalized() 还没弄懂。

2. BaseUnaryEdge：单节点边的模板类。边这一类都需要实现 computeError 计算误差和 linearizeOplus 计算雅克比矩阵。
3. BaseBinaryEdge：双节点边的模板类。下面以投影误差为例解释。由于代码量不大，部分就直接在代码中注释说明：

// 实例化时，按照 <维数，误差类，节点1类>
class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeSE3ProjectXYZOnlyPose(){}

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  // 计算误差，观测值和实际值之差，返回实例化时指定的误差类。
  void computeError()  {
    // 获取节点值计算误差。
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    Vector2d obs(_measurement);
    _error = obs-cam_project(v1->estimate().map(Xw));
  }

  bool isDepthPositive() {
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    return (v1->estimate().map(Xw))(2)>0.0;
  }

  // 计算雅克比矩阵
  virtual void linearizeOplus();

  Vector2d cam_project(const Vector3d & trans_xyz) const;

  Vector3d Xw;
  double fx, fy, cx, cy;
};

void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
  VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
  Vector3d xyz_trans = vi->estimate().map(Xw);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double invz = 1.0/xyz_trans[2];
  double invz_2 = invz*invz;

  // 投影模型导数计算。雅克比矩阵为 2(误差为2维) * 6（SE3 为 6 维）
  _jacobianOplusXi(0,0) =  x*y*invz_2 *fx;
  _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
  _jacobianOplusXi(0,2) = y*invz *fx;
  _jacobianOplusXi(0,3) = -invz *fx;
  _jacobianOplusXi(0,4) = 0;
  _jacobianOplusXi(0,5) = x*invz_2 *fx;

  _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
  _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
  _jacobianOplusXi(1,2) = -x*invz *fy;
  _jacobianOplusXi(1,3) = 0;
  _jacobianOplusXi(1,4) = -invz *fy;
  _jacobianOplusXi(1,5) = y*invz_2 *fy;
}

常用方法

• .setVertex()：设置关联的节点。
  eg. e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));设置第一个节点。
• .setMeasurement()：设置观测值，$\overrightarrow{z_k}$
• .setInformation()：设置信息矩阵，计算误差用。
• .setRobustKernel()：设置核方法，例如 Huber 核，提高鲁棒性。
• .setLevel()：设置边的级别，后文有详细解释。

使用步骤

跟把大象放进冰箱一样。

• 初始化 优化器

// 初始化优化器
g2o::SparseOptimizer optimizer;
// 设置求解算法
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

• 设置边和节点

构建上面的边和节点后，添加到图中。.addVertex() 和 .addEdge() 方法。

• 优化

optimizer.initializeOptimization(0);
optimizer.optimize(5);

Advance

剔除 Outliers

优化完之后，需要剔除掉外点然后再优化一次。在

optimizer.initializeOptimization(0);

会设置优化器的级别（此处为0）。优化器会优化最高到此级别的边。在 ORB_SLAM2 中

if(e->chi2()>7.815 || !e->isDepthPositive())
{
    e->setLevel(1);
}

将Outliers的级别设置为1，就被优化器忽略了。