Ceres详解（一） Problem类

引言

Ceres``是由Google开发的开源C++通用非线性优化库（项目主页），与g2o并列为目前视觉SLAM中应用最广泛的优化算法库（VINS-Mono中的大部分优化工作均基于Ceres完成）。Ceres中的有限边界最小二乘问题建模为以下形式：

$$\begin{array}{rl}& \underset{x}{\min }\frac{1}{2}\sum _i{\rho }_i\left(\Vert f_i\left(x_{i1},x_{i2},\cdots ,x_{ik}\right){\Vert }^2\right)\\ & s.t.l_j⩽x_j⩽u_j\end{array}$$

${\rho }_i\left(\Vert f_i\left(x_{i1},x_{i2},\cdots ,x_{ik}\right){\Vert }^2\right)$称为残差模块（residual block），${\rho }_i\left(\cdot \right)$称为损失函数（loss function）；$\{x_{i1},x_{i2},\cdots ,x_{ik}\}$称为参数模块（parameter blocks），$l_j$和$u_j$分别为参数模块的下界和上界，$f_i\left(\cdot \right)$为参数模块对应的代价函数（cost function）。当损失函数取单位函数${\rho }_i\left(x\right)=x$，下界$l_j=-\infty$，上界$u_j=\infty$，该问题即变为更为简介的无约束最小二乘问题。

Ceres的求解过程包括构建最小二乘和求解最小二乘问题两部分，其中构建最小二乘问题的相关方法均包含在Ceres::Problem类中，涉及的成员函数主要包括Problem::AddResidualBlock()和Problem::AddParameterBlock()。

Problem::AddResidualBlock( )

AddResidualBlock()顾名思义主要用于向Problem类传递残差模块的信息，函数原型如下，传递的参数主要包括代价函数模块、损失函数模块和参数模块。

ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function, 
										  LossFunction *loss_function, 
										  const vector<double *> parameter_blocks)
										  
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function, 
										  LossFunction *loss_function,
										  double *x0, double *x1, ...)

• 代价函数：包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式。AddResidualBlock( )函数会检测传入的参数模块是否和代价函数模块中定义的维数一致，维度不一致时程序会强制退出。代价函数模块的详解参见Ceres详解（二） CostFunction。
• 损失函数：用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等（完整的参数列表参见Ceres API文档）；该参数可以取NULL或nullptr，此时损失函数为单位函数。
• 参数模块：待优化的参数，可一次性传入所有参数的指针容器vector或依次传入所有参数的指针double*。

Problem::AddParameterBlock( )

用户在调用AddResidualBlock( )时其实已经隐式地向Problem传递了参数模块，但在一些情况下，需要用户显示地向Problem传入参数模块（通常出现在需要对优化参数进行重新参数化的情况）。Ceres提供了Problem::AddParameterBlock( )函数用于用户显式传递参数模块：

void Problem::AddParameterBlock(double *values, int size)

void Problem::AddParameterBlock(double *values, int size, LocalParameterization *local_parameterization)

其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式传递参数并没有太大区别。第二种函数原型则会额外传入LocalParameterization参数，用于重构优化参数的维数，这里我们着重讲解一下LocalParameterization类。

LocalParameterization

LocalParameterization类的作用是解决非线性优化中的过参数化问题。所谓过参数化，即待优化参数的实际自由度小于参数本身的自由度。例如在SLAM中，当采用四元数表示位姿时，由于四元数本身的约束（模长为1），实际的自由度为3而非4。此时，若直接传递四元数进行优化，冗余的维数会带来计算资源的浪费，需要使用Ceres预先定义的QuaternionParameterization对优化参数进行重构：

problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数

ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量

自定义LocalParameterization

LocalParaneterization本身是一个虚基类，详细定义如下。用户可以自行定义自己需要使用的子类，或使用Ceres预先定义好的子类。

class LocalParameterization {
 public:
  virtual ~LocalParameterization() {}
  //
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;//参数正切空间上的更新函数
  virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; //雅克比矩阵
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;//一般不用
  virtual int GlobalSize() const = 0; // 参数的实际维数
  virtual int LocalSize() const = 0; // 正切空间上的参数维数
};

上述成员函数中，需要我们改写的主要为GlobalSize()、ComputeJacobian()、GlobalSize()和LocalSize()，这里我们以ceres预先定义好的QuaternionParameterization为例具体说明，类声明如下：

class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
 public:
  virtual ~QuaternionParameterization() {}
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;
  virtual int GlobalSize() const { return 4; }
  virtual int LocalSize() const { return 3; }
};

• 可以看到，GlobalSize()的返回值为4，即四元数本身的实际维数；由于在内部优化时，ceres采用的是旋转矢量，维数为3，因此LocalSize()的返回值为3。
• 重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数x，用旋转矢量表示的增量delta，以及更新后的四元数x_plus_delta。函数首先将增量由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。

bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const {
  // 将旋转矢量转换为四元数形式
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
    double q_delta[4];
    q_delta[0] = cos(norm_delta);
    q_delta[1] = sin_delta_by_delta * delta[0];
    q_delta[2] = sin_delta_by_delta * delta[1];
    q_delta[3] = sin_delta_by_delta * delta[2];
    // 采用四元数乘法更新
    QuaternionProduct(q_delta, x, x_plus_delta);
  } else {
    for (int i = 0; i < 4; ++i) {
      x_plus_delta[i] = x[i];
    }
  }
  return true;
}

• ComputeJacobian函数给出了四元数相对于旋转矢量的雅克比矩阵计算方法，即${\mathbf{J}}_{4\times 3}=d\mathbf{q}/d\mathbf{v}=d{\left[q_w,q_x,q_y,q_z\right]}^T/d\left[\right[x,y,z]$，对应Jacobian维数为4行3列，存储方式为行主序。

bool QuaternionParameterization::ComputeJacobian(const double* x,
                                                 double* jacobian) const {
  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
  return true;
}

ceres预定义LocalParameterization

除了上面提到的QuaternionParameterization外，ceres还提供下述预定义LocalParameterization子类：

• EigenQuaternionParameterization：除四元数排序采用Eigen的实部最后外，与QuaternionParameterization完全一致；
• IdentityParameterizationconst：LocalSize与GlobalSize一致，相当于不传入LocalParameterization；
• SubsetParameterization：GlobalSize为2，LocalSize为1，用于第一维不需要优化的情况；
• HomogeneousVectorParameterization：具有共面约束的空间点；
• ProductParameterization：7维位姿变量一同优化，而前4维用四元数表示的情况（这里源文档只举了一个例子，具体用法有待深化）；

其他成员函数

Probelm还提供了其他关于ResidualBlock和ParameterBlock的函数，例如获取模块维数、判断是否存在模块、存在的模块数目等，这里只列出几个比较重要的函数，完整的列表参见ceres API：

// 设定对应的参数模块在优化过程中保持不变
void Problem::SetParameterBlockConstant(double *values)
// 设定对应的参数模块在优化过程中可变
void Problem::SetParameterBlockVariable(double *values)
// 设定优化下界
void Problem::SetParameterLowerBound(double *values, int index, double lower_bound)
// 设定优化上界
void Problem::SetParameterUpperBound(double *values, int index, double upper_bound)
// 该函数紧跟在参数赋值后，在给定的参数位置求解Problem，给出当前位置处的cost、梯度以及Jacobian矩阵；
bool Problem::Evaluate(const Problem::EvaluateOptions &options, 
					   double *cost, vector<double>* residuals, 
					   vector<double> *gradient, CRSMatrix *jacobian)