Ceres详解（一） Problem类
- 引言
- Problem::AddResidualBlock( )
- Problem::AddParameterBlock( )
- LocalParameterization
- 自定义LocalParameterization
- ceres预定义LocalParameterization
- 其他成员函数
引言
Ceres``是由Google开发的开源C++通用非线性优化库（项目主页），与g2o并列为目前视觉SLAM中应用最广泛的优化算法库（VINS-Mono中的大部分优化工作均基于Ceres完成）。Ceres中的有限边界最小二乘问题建模为以下形式：
称为残差模块（residual block），
称为损失函数（loss function）；
称为参数模块（parameter blocks），
和
分别为参数模块的下界和上界，
为参数模块对应的代价函数（cost function）。当损失函数取单位函数
，下界
，上界
，该问题即变为更为简介的无约束最小二乘问题。
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*>或依次传入所有参数的指针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函数给出了四元数相对于旋转矢量的雅克比矩阵计算方法，即，对应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)
