使用Ceres进行slam必须要弄清楚的几个类和函数

Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。
在之前的博客说了,图优化的本质就是一个非线性优化问题.所以ceres刚好适用图优化问题的解决.
在进行特征点匹配后进行迭代的优化最优变换位姿时也可以使用ceres.

ceres简介

Ceres可以解决边界约束鲁棒非线性最小二乘法优化问题。这个概念可以用以下表达式表示：

这一表达式在工程和科学领域有非常广泛的应用。比如统计学中的曲线拟合，或者在计算机视觉中依据图像进行三维模型的构建等等。

注意这个公式里面各模块有几个特殊的概念要熟知,涉及到具体的使用:

残差块（ResidualBlock）:

这一部分被称为残差块（ResidualBlock）.

代价函数（CostFunction）:

这一部分被称为代价函数（CostFunction）.

参数块（ParameterBlock）:

代价函数依赖于一系列参数,这一系列参数（均为标量）称为参数块（ParameterBlock）.当然参数块中也可以只含有一个变量

上下边界:

lj和uj是xj的上下边界.

损失函数（LossFunction）:

pi是损失函数（LossFunction）.按照损失函数的是一个标量函数，其作用是减少异常值（Outliers）对优化结果的影响。其效果类似于对函数的过滤。

ceres的使用流程

1.构建代价函数(cost function)
2.通过代价函数构建待求解的优化问题
3.配置求解器参数并求解问题

ceres必须要知道的类和函数

class LossFunction

class LossFunction 损失函数
最小二乘问题的输入数据可能包含异常值(错误测量得到的)，使用损失函数减少这部分数据的影响。

比如说当一个移动相机的场景中,街道上有消防栓和汽车,当图像的处理算法把消防栓的尖和汽车的前灯匹配在了一起,那么如果不做任何处理,则会导致ceres为使这个错误的大的误差减小,而是优化结果偏离正确位置.

LossFunction可以让大的残差的权重降低,从而对 最终的优化结果没有太大的影响.

class LossFunction {
    
 public:
  virtual void Evaluate(double s, double out[3]) const = 0;
};

LossFunction的类,关键的函数就是 LossFunction::Evaluate()
一个非负的参数s,计算输出

Ceres包含了几种定义好的损失函数,都是没有缩放的.具体效果如下图所示:

图中红色的就是没有经过损失函数的,y=x*x.蓝色的是HuberLoss,值低于正常值,并且x越大,效果越明显.
正常的是:

HuberLoss是:

SoftLOneLoss是:

CauchyLoss是:

ArctanLoss是:

TolerantLoss是:

用定义好的损失函数使用也很简单.例如:

ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);

定义ceres 的 损失函数 0.1代表 残差大于0.1的点 ,则权重降低,具体效果看上面的公式. 小于0.1 则认为正常,不做特殊的处理
定义好后,在添加残差

LocalParameterization

LocalParameterization 本地参数
在许多优化问题中，尤其是传感器融合问题中，必须对存在于称为流形的空间中的量进行建模，例如由四元数表示的传感器的旋转/方向。

Ceres定义了一些特殊的参数,对于slam,用的更多的就是旋转的四元数
QuaternionParameterization
EigenQuaternionParameterization
定义两个主要的原因就是Eigen的存储四元数的方式和一般的不同,Eigen是x,y,z,w,实数部分的w放在最后,一般的则是:w,x,y,z.

使用

double para_q[4] = {
    0, 0, 0, 1};
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
problem.AddParameterBlock(para_q, 4, q_parameterization);

class problem

problem类就是代表者具有双边约束的最小二乘问题
为了创建一个最小二乘问题,需要使用
Problem::AddResidalBlock() 添加残差模块
Problem::AddParameterBlock() 添加参数模块
这两个方法

举个例子,一个问题包含三个参数模块,尺寸分别是3,4,5,两个残差模块尺寸分别是2和6

double x1[] = {
     1.0, 2.0, 3.0 };
double x2[] = {
     1.0, 2.0, 3.0, 5.0 };
double x3[] = {
     1.0, 2.0, 3.0, 6.0, 7.0 };

Problem problem;
problem.AddResidualBlock(new MyUnaryCostFunction(...), x1);
problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3);

方法Problem::AddResidualBlock(),和名字一样,功能就是添加参数模块到problem中,这个方法必须有参数CostFunction和可选参数LossFunction,这个方法就连接了CostFunction和参数模块.
CostFunction 具有它希望的参数块尺寸的信息.
这个函数检查这些是否与 parameter_blocks 中列出的参数块的大小相匹配 .如果检测到错误的匹配,程序会终止.
LossFunction 可以有,也可以没有

可以使用Problem::AddParameterBlock() 这个方法来添加参数模块,这个会添加一次参数尺寸的检测.将参数块显式添加到问题中。 还允许将 Manifold 对象与参数块相关联。
这个函数可以用带LocalParameterization的参数,也可以不带.

problem.AddParameterBlock(para_q, 4, q_parameterization);// 添加四元数的参数块
problem.AddParameterBlock(para_t, 3);//添加平移的参数块

声明的时候一般这样:

ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);

先声明一个ceres::Problem::Options,然后再用Options初始化problem

class CostFunction

代价函数CostFunction 负责计算残差向量和雅克比矩阵.
代价函数依赖参数块

其内部定义是这样的

class CostFunction {
    
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) = 0;
  const vector<int32>& parameter_block_sizes();
  int num_residuals() const;

 protected:
  vector<int32>* mutable_parameter_block_sizes();
  void set_num_residuals(int num_residuals);
};

这部分不用太管,因为使用的时候用其它类定义的这个类的内部
定义 CostFunction 或 SizedCostFunction 可能容易出错，尤其是在计算导数时。 为此，Ceres 提供了自动微分。

class AutoDiffCostFunction

定义 CostFunction 或 SizedCostFunction 可能容易出错，尤其是在计算导数时。 为此，Ceres 提供了自动微分。

template <typename CostFunctor,
       int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
       int... Ns>          // Size of each parameter block
class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns> {
    
 public:
  AutoDiffCostFunction(CostFunctor* functor, ownership = TAKE_OWNERSHIP);
  // Ignore the template parameter kNumResiduals and use
  // num_residuals instead.
  AutoDiffCostFunction(CostFunctor* functor,
                       int num_residuals,
                       ownership = TAKE_OWNERSHIP);
};

得到一个可以自动求导的代价函数,必须定义一个类或者结构体在里面重载运算符,在里面实现用参数模板计算代价函数,重载的运算符必须在最后一个参数里存入计算结果,并且返回true.

举个例子,要计算 一个 代价函数是 e= k - xTy.
x和y是二维的向量,k是一个不变的参数.
那么可以定义一个这样的类

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

  template <typename T>
  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_;
};

在重载运算符的定义里面.参数x和y在前面,如果有更多的输入参数,则继续可以排在y后面,输出也就是残差放在最后一个参数,

给定这个类的定义之后，它的自动微分代价函数可以定义如下:

CostFunction* cost_function
    = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
        new MyScalarCostFunctor(1.0));              ^  ^  ^
                                                    |  |  |
                        Dimension of residual ------+  |  |
                        Dimension of x ----------------+  |
                        Dimension of y -------------------+

上面的1,2,2 就是注释的那样,计算1维的残差,2个2维的优化量