Ceres Solver：Google高效的非线性优化库
Ceres是由Google开发的开源C++通用非线性优化库（项目主页），与g2o并列为目前视觉SLAM中应用最广泛的优化算法库。Ceres中的有限边界最小二乘问题建模为以下形式：
可以发现，Ceres计算cost的公式与g2o有所不同，因此同样的优化问题，最终的cost一般是g2o的一半。
官方样例参考：
Ceres Solver - 随笔分类 - JJ_S - 博客园Ceres中的优化需要四步：
- 构建优化问题Problem类
- 构建优化的残差函数CostFunction
- 最小二乘问题构建，在每次获取到数据后添加残差块
- 求解最小二乘问题
Ceres的求解过程包括构：建立最小二乘和求解最小二乘问题两部分，其中构建最小二乘问题的相关方法均包含在Ceres::Problem类中，涉及的成员函数主要包括：
- Problem::AddResidualBlock()
- Problem::AddParameterBlock()
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*。
用户在调用 AddResidualBlock() 时其实已经隐式地向 Problem 传递了参数模块，但在一些情况下，需要用户显式地向 Problem 传入参数模块（通常出现在需要对优化参数进行重新参数化的情况，因为这个时候，优化的参数已经变了）。
参考：
Ceres 详解（一） Problem类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)
与其他非线性优化工具包一样，ceres的性能很大程度上依赖于导数计算的精度和效率。这部分工作在ceres中称为 CostFunction，ceres提供了许多种 CostFunction模板，较为常用的包括以下三种：
1、自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式，最常用的求导方式。
2、数值导数（NumericDiffCostFunction）：当无法分析或使用自动微分计算导数时，应使用数值微分。通常情况下，比如用一个外部库或函数时；但精度和计算效率不如自动导数和解析导数，因此不到不得已，官方并不建议使用该方法。当使用数值微分时，至少使用中值差分，如果执行时间不是问题，或者目标函数难以确定良好的静态相对步长，建议使用Ridders方法。
就是梯度法。Ceres 数值导数 （NumericDerivatives）进阶
3、解析导数（SizedCostFunction）：当导数存在闭合解析形式时使用，用于可基于SizedCostFunction基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则同样不建议使用
手动推导，效率比AutoDiffCostFunction略高。ceres解析导数（Analytic Derivatives）进阶
可以看出，ceres官方极力推荐用户使用自动求导方式AutoDiffCostFunction，这里也主要以AutoDiffCostFunction为例说明。AutoDiffCostFunction为模板类，构造函数如下：
// CostFunctor: 仿函数（functor）类型
// residualDim: 残差维数
// paramDim: 参数维数（可以有多个）
// CostFunctor: 仿函数指针
ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(CostFunctor* functor);
ceres库的自动求解雅克比的原理
Ceres 自动求导的核心是Ceres自有的Jet变量与运算符的重载（一些常用函数的求导、链式求导），计算结果的同时，计算了导数，其他没实现的可微函数用泰勒展开式中的前k项。
Ceres 自动求导解析-从原理到实践
Ceres的自动求导实现原理剖析_ceres jet_linzs.online的博客-CSDN博客
Ceres Jet型-CSDN博客
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
template <typename T>
bool quat_cross_vec(const T *q, const double *t, T *res)
{
Quaternion<T> q_curr{q[2], q[3], q[0], q[1]};
QuaternionToAngleAxis(q_curr.coeffs().data(), res);
return true;
}
int main()
{
Vector3d vec=Vector3d::Random();
Quaterniond Q_tmp=Quaterniond::UnitRandom().normalized();
Matrix<ceres::Jet<double, 4>, 3, 1> res;
Quaternion<ceres::Jet<double, 4>> Q;
Q.x()=ceres::Jet<double, 4>(Q_tmp.x(), 0);
Q.y()=ceres::Jet<double, 4>(Q_tmp.y(), 1);
Q.z()=ceres::Jet<double, 4>(Q_tmp.z(), 2);
Q.w()=ceres::Jet<double, 4>(Q_tmp.w(), 3);
quat_cross_vec(Q.coeffs().data(), vec.data(), res.data());
printf("%f %f %f %f\
", res[0].v[0], res[0].v[1], res[0].v[2], res[0].v[3]);
printf("%f %f %f %f\
", res[1].v[0], res[1].v[1], res[1].v[2], res[1].v[3]);
printf("%f %f %f %f\
\
", res[2].v[0], res[2].v[1], res[2].v[2], res[2].v[3]);
return 0;
}ceres自动求导的问题
ceres自动求导，如果使用到四元数(或其他旋转类型)，可以使用<ceres/rotation.h>中的函数，不然可能会有较大误差
(1)其中修正了Eigen中的一些问题，例如: Quaternion.toRotationMatrix()/Quaternion.inverse()
(2)比如QuaternionToAngleAxis/QuaternionToRotation等
(3)可以使用sophus库
(4)可以使用解析导数，提高精度仿函数的本质为结构体struct或者类class，由于重载了()运算符，使得其能够具有和函数一样的调用行为，因此被称为仿函数。
由于是仿函数，operator()运算符必须被重载。
参考：
Ceres详解（二） CostFunctionceres鲁棒核函数总结：HuberLoss、CauchyLoss、SoftLOneLoss 、TrivialLoss。ceres中的loss函数实现探查，包括Huber，Cauchy，Tolerant图像实现及源码_卫少东的博客-CSDN博客_huber ceresModeling Non-linear Least Squares
我们构建一个ceres::Problem对象，只做一件事：利用AddResidualBlock向其中添加残差模块。
// 构建BA问题
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); i++){
// 调用工厂函数create()构建CostFunction对象
ceres::CostFunction* costfunctor =
ReprojectionError3D::create(observations[0 + 2 * i], observations[1 + 2 * i]);
// If enabled use Huber's loss function.
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
// 添加该次观测对应的残差，传入参数误差仿函数，单位损失函数，以及该观测对应的相机位姿和空间点坐标
problem.AddResidualBlock(costfunctor,
loss_function,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
}
至此，一个最小二乘BA问题边构建完成了，接下来我们使用ceres::Solve函数求解该问题。
ceres::Solve函数是Ceres求解最小二乘问题的核心函数，函数原型如下：
void Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary)
函数接受的三个参数分别为：求解选项Solver::Options、求解问题Problem以及求解报告Solver::Summary。
其中Problem类我们已经在第一讲详细介绍过；
Solver::Summary只用于存储求解过程中的相关信息，并不影响求解器性能；
Solver::Options则是Ceres求解的核心，包括消元顺序、分解方法、收敛精度等在内的求解器所有行为均由Solver::Options控制。
Solver::Summary包含了求解器本身和求解中各变量的信息，许多成员函数与Solver::Options一致，详细列表同样请参阅API文档，这里只给出另外两个常用的成员函数：
- BriefReport()：输出单行的简单总结；
- FullReport()：输出多行的完整总结。
现在我们来看本例中的Solver::Summary的使用：
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\
";// 输出完整的报告
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和线程数num_threads，如果发现最后的求解精度或求解效率不能满足要求，应首先尝试更换这两个参数。
ceres solver里面定义了7种线性求解器（默认为DENSE_QR），分别是：
- DENSE_QR：对于有一百多个优化变量或不到1000个残差项的小优化问题，如果Jacobian是相对稠密的，那么使用QR分解；
- DENSE_NORMAL_CHOLESKY & SPARSE_NORMAL_CHOLESKY：Cholesky分解，用于具有稀疏性的大规模非线性最小二乘问题求解：ceres之dense cholesky和sparse cholesky求解器-CSDN博客；
这两种方法用来解决大型的优化问题，区别主要是使用的数据结构是否是稀疏矩阵，由于大型优化问题中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问题；
注意：系数矩阵如果不满足分解方法对应前提条件（可逆、对称、正定等）时，可能会求解失败。
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.842900e+07 0.00e+00 2.04e+06 0.00e+00 0.00e+00 1.00e+04 0 3.28e-02 2.28e-01
1 1.449093e+06 1.70e+07 1.75e+06 0.00e+00 1.84e+00 3.00e+04 1 9.27e-02 3.20e-01
2 5.848543e+04 1.39e+06 1.30e+06 1.55e+03 1.87e+00 9.00e+04 1 7.70e-02 3.97e-01
3 1.581483e+04 4.27e+04 4.98e+05 4.98e+02 1.29e+00 2.70e+05 1 7.86e-02 4.76e-01
4 1.251823e+04 3.30e+03 4.64e+04 9.96e+01 1.11e+00 8.10e+05 1 7.59e-02 5.52e-01
5 1.240936e+04 1.09e+02 9.78e+03 1.33e+01 1.42e+00 2.43e+06 1 7.36e-02 6.26e-01
6 1.237699e+04 3.24e+01 3.91e+03 5.04e+00 1.70e+00 7.29e+06 1 8.11e-02 7.07e-01
7 1.236187e+04 1.51e+01 1.96e+03 3.40e+00 1.75e+00 2.19e+07 1 7.33e-02 7.80e-01
8 1.235405e+04 7.82e+00 1.03e+03 2.40e+00 1.76e+00 6.56e+07 1 7.29e-02 8.53e-01
9 1.234934e+04 4.71e+00 5.04e+02 1.67e+00 1.87e+00 1.97e+08 1 7.71e-02 9.30e-01
10 1.234610e+04 3.24e+00 4.31e+02 1.15e+00 1.88e+00 5.90e+08 1 7.44e-02 1.00e+00
11 1.233725e+04 9.50e-02 1.19e+02 7.07e-01 1.99e+00 8.47e+15 1 6.97e-02 2.09e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0126 01:37:00.497905 8018 levenberg_marquardt_strategy.cc:123] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
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);
该选型默认为false，即根据vlog设置等级的不同，只会在向STDERR中输出错误信息；若设置为true则会向程序的运行终端输出优化过程的所有信息，根据所设置优化方法的不同，输出的参数亦不同。
- 信赖域方法
- cost：当前目标函数的数值
- cost_change：当前参数变化量引起的目标函数变化
- |gradient|：当前梯度的模长
- |step|：参数变化量
- tr_ratio：目标函数实际变化量和信赖域中目标函数变化量的比值；
- tr_radius：信赖域半径；
- ls_iter：线性求解器的迭代次数，对于直接/因子分解求解器该数值永远是1；对于迭代求解器，该数值等于求解共轭梯度的迭代次数
- iter_time：当前迭代耗时；
- total_time：优化总耗时；
- 线性搜索方法
- f：当前目标函数的数值
- d：当前参数变化量引起的目标函数变化
- g：当前梯度的模长
- h：参数变化量
- s：线性搜索最优步长；
- it：当前迭代耗时；
- tt：优化总耗时；
现在我们来看本例中的求解器设置：
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR; //使用DENSE_SCHUR分解
options.minimizer_progress_to_stdout = true; // 输出优化过程信息
// 函数原型
bool ceres::Problem::HasParameterBlock(const double *values) const;
// Is the given parameter block present in this problem or not?
// 函数原型
void ceres::Problem::SetParameterLowerBound(double *values, int index, double lower_bound)
void ceres::Problem::SetParameterUpperBound(double *values, int index, double upper_bound)
//Set the lower/upper bound for the parameter at position "index".参考：
Ceres Solver - 随笔分类 - JJ_S - 博客园LOAM中Ceres的使用分析_bug大湿的博客-CSDN博客haichuan：ceres-solver中的自动求导haichuan：ceres solver里的线性求解器(1)Ceres 详解（一） Problem类Ceres详解（二） CostFunctionCeres详解（三）最小二乘问题构建与求解_他人是一面镜子，保持谦虚的态度的博客-CSDN博客_ceres summary
