Ceres

Ceres是由Google开发的开源C++通用非线性优化库，与g2o并列为目前视觉SLAM中应用最广泛的优化算法库。Ceres可以解决如下形式的有限边界最小二乘问题：
$$\begin{array}{rl}& \underset{x}{\min }\frac{1}{2}\sum _i{\rho }_i\left({\Vert f_i\left(x_{i_1},x_{i_2},\dots ,x_{ik}\right)\Vert }^2\right)\\ & s.t.l_j\le x_j\le u_j\end{array}$$
其中 ${\rho }_i\left({\Vert f_i\left(x_{i_1},x_{i_2},\dots ,x_{ik}\right)\Vert }^2\right)$称为残差模块(residual bolck)， ${\rho }_i(\cdot )$称为损失函数(loss function)，损失函数是标量函数，用于减少异常值对非线性最小二乘问题求解的影响。； { x i 1 , x i 2 , … , x i k } \{x_{i_{1}},x_{i_{2}},\ldots ,x_{ik}\} {xi1​​,xi2​​,…,xik​}称为参数模块(parameter blocks)， $l_j$和 $u_j$分别为参数模块的上界和下界， $f_i(\cdot )$为参数模块对应的代价函数(cost function)。

可以发现，Ceres计算cost的公式与g2o有所不同，因此同样的优化问题，最终的cost一般是g2o的一半。

Ceres中的优化需要四步：

1. 构建优化问题Problem类
2. 构建优化的残差函数CostFunction
3. 最小二乘问题构建，在每次获取到数据后添加残差块
4. 求解最小二乘问题

Problem类

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

1. Problem::AddResidualBlock()
2. Problem::AddParameterBlock()

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*。

AddParameterBlock()

用户在调用 AddResidualBlock() 时其实已经隐式地向 Problem 传递了参数模块，但在一些情况下，需要用户显式地向 Problem 传入参数模块（通常出现在需要对优化参数进行重新参数化的情况，因为这个时候，优化的参数已经变了）。

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

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

// 设定对应的参数模块在优化过程中保持不变
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)

CostFunction类

与其他非线性优化工具包一样，ceres的性能很大程度上依赖于导数计算的精度和效率。这部分工作在ceres中称为 CostFunction，ceres提供了许多种 CostFunction模板，较为常用的包括以下三种：

1. 自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式，最常用的求导方式。
2. 数值导数（NumericDiffCostFunction）：由用户手动编写导数的数值求解形式，通常在残差函数的计算使用无法直接调用的库函数，导致调用AutoDiffCostFunction类构建时使用；但手动编写的精度和计算效率不如模板类，因此不到不得已，官方并不建议使用该方法。
3. 解析导数（Analytic Derivatives）：当导数存在闭合解析形式时使用，用于可基于CostFunciton基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则同样不建议使用。详细内容参考

手动推导，效率比AutoDiffCostFunction略高。

AutoDiffCostFunction

可以看出，ceres官方极力推荐用户使用自动求导方式AutoDiffCostFunction，这里也主要以AutoDiffCostFunction为例说明。AutoDiffCostFunction为模板类，构造函数如下：

// CostFunctor: 仿函数（functor）类型
// residualDim: 残差维数
// paramDim: 参数维数（可以有多个）
// functor: 仿函数指针
ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(CostFunctor* functor);

CostFunctor仿函数

仿函数的本质为结构体struct或者类class，由于重载了()运算符，使得其能够具有和函数一样的调用行为，因此被称为仿函数。
由于是仿函数，operator()运算符必须被重载。

求解最小二乘问题

ceres::Solve函数是Ceres求解最小二乘问题的核心函数，函数原型如下：

void Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary)

函数接受的三个参数分别为：求解选项Solver::Options、求解问题Problem以及求解报告Solver::Summary。

• Solver::Summary只用于存储求解过程中的相关信息，并不影响求解器性能；

• Solver::Options则是Ceres求解的核心，包括消元顺序、分解方法、收敛精度等在内的求解器所有行为均由Solver::Options控制。

Solver::Summary

Solver::Summary包含了求解器本身和求解中各变量的信息，许多成员函数与Solver::Options一致，详细列表同样请参阅API文档，这里只给出另外两个常用的成员函数：

• BriefReport()：输出单行的简单总结；
• FullReport()：输出多行的完整总结。
  现在我们来看本例中的Solver::Summary的使用：

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";// 输出完整的报告

Solver::Options

Solver::Options含有的参数种类繁多，API文档中对于每个参数的作用和意义都给出了详细的说明。由于在大多数情况下，绝大多数参数我们都会使用Ceres的默认设置，这里只列出一些常用或较为重要的参数。

• minimizer_type：迭代求解方法，可选线搜索方法（LINEAR_SEARCH）或信赖域方法（TRUST_REGION），默认为TRUST_REGION方法；由于大多数情况我们都会选择LM或DOGLEG方法，该选项一般直接采用默认值；
• trust_region_strategy_type：信赖域策略，可选LEVENBERG_MARQUARDT或DOGLEG，默认为LEVENBERG_MARQUARDT；
• linear_solver_type：信赖域方法中求解线性方程组所使用的求解器类型，默认为DENSE_QR；
• linear_solver_ordering：线性方程求解器的消元顺序，默认为NULL，即由Ceres自行决定消元顺序；在以BA为典型代表的，对消元顺序有特殊要求的应用中，可以通过成员函数reset设定消元顺序，稍后将详细说明；
• min_linear_solver_iteration/max_linear_solver_iteration：线性求解器的最小/最大迭代次数，默认为0/500，一般不需要更改；
• max_num_iterations：求解器的最大迭代次数；
• max_solver_time_in_seconds：求解器的最大运行秒数；
• num_threads：Ceres求解时使用的线程数，在老版本的Ceres中还有一个针对线性求解器的线程设置选项num_linear_solver_threads，最新版本的Ceres中该选项已被取消；虽然为了保证程序的兼容性，用户依旧可以设置该参数，但Ceres会自动忽略该参数，并没有实际意义；
• minimizer_progress_to_stdout：是否向终端输出优化过程信息，具体内容稍后详细说明；

linear_solver_type

在实际应用中，上述参数中对最终求解性能最大的就是线性方程求解器类型linear_solver_type和线程数num_threads，如果发现最后的求解精度或求解效率不能满足要求，应首先尝试更换这两个参数。

ceres solver里面定义了7种线性求解器（默认为DENSE_QR），分别是：

• DENSE_QR：对于有一百多个优化变量或不到1000个残差项的小优化问题，如果Jacobian是相对稠密的，那么使用QR分解；
• DENSE_NORMAL_CHOLESKY & SPARSE_NORMAL_CHOLESKY：Cholesky分解，用于具有稀疏性的大规模非线性最小二乘问题求解：ceres之dense cholesky和sparse cholesky求解器-优快云博客；
  这两种方法用来解决大型的优化问题，由于大型优化问题中Jacobian经常有一堆0，如果非常稀疏我们用SPARSE_NORMAL_CHOLESKY，否则用
• DENSE_NORMAL_CHOLESKY。 对于BA问题，可以使用SPARSE_NORMAL_CHOLESKY来求解。
• DENSE_SCHUR & SPARSE_SCHUR：SCHUR分解，用于BA问题求解；
  由于BA的特殊结构，我们可以使用这两种方法，其中SPARSE_SCHUR效率更高。
• CGNR：使用共轭梯度法求解稀疏方程；
• ITERATIVE_SCHUR：使用共轭梯度SCHUR求解BA问题；

linear_solver_ordering

Ceres消元顺序的设置由linear_solver_ordering的reset函数完成，该函数接受参数为ParameterBlockOrdering对象。

该对象将所有待优化参数存储为带标记（ID）的组（Group），ID小的Group在求解线性方程的过程中会被首先消去。因此，我们需要做的第一个工作是调用其成员函数AddElementToGroup将参数添加到对应ID的Group中，函数原型为：

bool ParameterBlockOrdering::AddElementToGroup(const double *element, const int group)

接收的元素为变量数组的指针；组ID为非负整数，最小为0，如果该Id对应的Group不存在，则Ceres会自动创建。
下面我们来看一个BA中的例子：

ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering();

// set all points in ordering to 0
for(int i = 0; i < num_points; i++){
    ordering->AddElementToGroup(points + i * point_block_size, 0);
}
// set all cameras in ordering to 1
for(int i = 0; i < num_cameras; i++){
    ordering->AddElementToGroup(cameras + i * camera_block_size, 1);
}

该例子中，所有路标点被分到了ID = 0组，而所有相机位姿被分到了ID = 1组，因此在线性方程组的求解中，所有路标点会变首先SCHUR消元。

接下来，我们就可以使用reset函数制定线性求解器的消元顺序了：

// set ordering in options
options->linear_solver_ordering.reset(ordering);

QuaternionManifold

QuaternionManifold与Manifold的扰动模型不同，QuaternionManifold为全局扰动模型
$$\begin{array}{rl}⊞(x,\Delta )& =\exp (\Delta )\otimes x\\ \boxminus (y,x)& =\log (y\otimes x^{-1})\end{array}$$