引言
Ceres 是由Google开发的开源C++通用非线性优化库（项目主页），与g2o并列为目前视觉SLAM中应用最广泛的优化算法库
（VINS-Mono中的大部分优化工作均基于Ceres完成）。Ceres中的有限边界最小二乘问题建模为以下形式：
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; }
};
1、可以看到，GlobalSize() 的返回值为4，即四元数本身的实际维数；由于在内部优化时，
ceres采用的是旋转矢量，维数为3，因此 LocalSize() 的返回值为3。
2、重载的 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;
}
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)
扫一扫
318
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
