Skip to main content

非线性优化

Published: 2020-01-06 | Lastmod: 2020-01-29

Table of contents
  1. 优化方法比较
  2. g2o
  3. g2o在ORB-SLAM2中的用法
  4. Ceres
  5. Ceres在VINS-MONO中的用法

优化方法比较 #

方法
 特点
梯度下降法 \(x_{i+1}=x_i-\eta \frac{\partial f}{\partial x}\)，效率高，但可能收敛慢
牛顿法 二阶泰勒展开，计算Hessian矩阵耗时，离初值远不收敛，极小值处接近二次函数
高斯-牛顿法 仅适用于最小二乘问题，以Jacobian矩阵\(J^TJ\)代替\(H\)，离初值远不收敛，\(J^TJ\)近奇异不收敛
L-M法 \(\left(H+\lambda I\right)\Sigma=-J^Tr\)，残差增大时，放大\(\lambda\)，成为梯度下降；残差减小时，减小\(\lambda\)，成为高斯-牛顿

g2o #

g2o在ORB-SLAM2中的用法 #

Ceres #

http://ceres-solver.org/nnls_modeling.html

对于以下代价函数：

\[ \frac{1}{2} \sum_i \rho_i(\parallel f_i(x_{i1}, x_{i2}, \cdots, x_{ik}) \parallel ^2) \]

在Ceres中，\(\rho\)为loss function，用于减弱外点的影响；\(f\)为cost function，定义了残差的计算方式；\((x_{i1}, x_{i2}, \cdots, x_{ik})\)为parameter block，定义了需要估计的变量；\(\rho_i(\parallel f_i(x_{i1}, x_{i2}, \cdots, x_{ik}) \parallel ^2)\)为residual block，定义了一个残差块。

AddResidualBlock

problem.AddResidualBlock(costfunction, lossfunction, x1, x2 ... xn)
其中，\(x_1, x_2, \cdots x_n\)要与costfunction中的变量维数要对应起来。

costfunction有两种典型的定义方式：

  1. 自己实现残差、雅可比
template
class SizedCostFunction : public CostFunction {
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const = 0;
};

这种情况下，需要指定残差维数、参数块维数，并自己实现Evaluate()，计算cost和jacobian。

  1. 自己实现残差即可
template 
class AutoDiffCostFunction : public
SizedCostFunction {
 public:
  explicit AutoDiffCostFunction(CostFunctor* functor);
  // Ignore the template parameter kNumResiduals and use
  // num_residuals instead.
  AutoDiffCostFunction(CostFunctor* functor, int num_residuals);
};

这种情况下，不再需要自己计算jacobian。只需要定义CostFunctor，计算残差，实现operator()函数。例如：

class MyScalarCostFunctor {
  MyScalarCostFunctor(double k): k_(k) {}

  template 
  bool operator()(const T* const x , const T* const y, T* e) const {
    e[0] = k_ - x[0] * y[0] - x[1] * y[1];
    return true;
  }

 private:
  double k_;
};

AddParameterBlock

ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);

Sometimes the parameters \(x\) can overparameterize a problem. In that case it is desirable to choose a parameterization to remove the null directions of the cost.

Ceres在VINS-MONO中的用法 #

Factor

VINS 中用 IMUFactor 表示 IMU 的 cost function，用 ProjectionFactor 表示图像重投影误差的 cost function。其中定义了对各个参数块的残差、雅可比。

MarginalizationFactor 表达了先验残差的更新方式。在Evaluate()函数中，实现了如下更新：

image

ResidualBlockInfo

为了进行 Schur，在 cost function 外面包了一层 ResidualBlockInfo。因为在原生Ceres中，一旦调用 problem.AddResidualBlock(costfunction, lossfunction, x1, x2 ... xn)，便不再能干预内部处理流程。

在这个自定义的 ResidualBlockInfo 的 Evaluate() 中，调用了每个 cost function 原生的 Evaluate()，获得残差、雅可比，然后将 loss function 的功能也在此调用，组装好以后，留给 MarginalizationInfo 做进一步处理。

MarginalizationInfo

addResidualBlockInfo() 处理优化变量、待边缘化变量

preMarginalize()

marginalize() 并行化线程，进行舒尔补计算


Next: SLAM for Dummies
Previous: VINS-MONO