Ceres详解（三）最小二乘问题构建与求解

最新推荐文章于 2024-05-26 21:57:55 发布

yuntian_li

最新推荐文章于 2024-05-26 21:57:55 发布

阅读量6.9k

点赞数 14

分类专栏： Ceres 文章标签： ceres slam

本文链接：https://blog.csdn.net/weixin_43991178/article/details/100568128

版权

Ceres 专栏收录该内容

3 篇文章 26 订阅

订阅专栏

Ceres详解（三）

构建最小二乘问题
求解最小二乘问题

在前两讲中，我们研究了Ceres中代表最小二乘问题的 Problem类以及代表误差函数的类 CostFunction，本讲我们以Bundle Adjustment为例，研究如何利用这两个类构建最小二乘问题，并使用 ceres::Solve()函数求解。

例子中使用的数据集为Bundle Adjustment in the Large数据集，完整的示例代码参见github。

构建最小二乘问题

首先我们需要构建一个BALProblem对象用于数据集的读取和存储，该类的源代码位于bal_problem.h和bal_problem.cpp中，本例中我们需要用到的成员函数说明如下：

LoadFile(const char* filename)：读取数据集文件；
observation()：返回包含所有观测值的数组，数组内容不可更改；
num_observations()：返回数据集总的观测次数；
mutable_camera_for_observation(int i)：返回第i次观测时对应的相机位姿，数组内容可更改；
mutable_point_for_observation(int i)：返回第i次观测时对应的空间点位置，数组内容可更改；

这里需要说明下，BAL数据集中的1次观测定义为一个相机位姿+一个空间点3D坐标+空间点2D像素坐标。

BALProblem bal_problem;// 创建BALProblem对象
// 由命令行给定的路径读取数据集文件
if(!bal_problem.LoadFile(argv[1])){
    cerr << "error loading dataset file " << argv[1] << endl;
    return 1; 
}
// 获取数据集的观测量信息，在ba问题中为二维像素坐标[u, v]
const double* observations = bal_problem.observations();

随后我们构建一个ceres::Problem对象，并利用AddResidualBlock向其中添加残差模块。这里由于数据集本身使用的位姿表示即旋转矢量，不需要使用LocalParameterization进行局部参数重构，因此并没有使用AddParameterBlock()显式传递参数。
在后续VINS-Mono的代码分析中我们将看到，由于使用的优化参数为四元数，需要使用LocalParameterization将四元数重构为三维旋转矢量进行优化和更新，就必须使用AddParameterBlock()显式传递参数。

 // 构建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]);
    // 添加该次观测对应的残差，传入参数误差仿函数，单位损失函数，以及该观测对应的相机位姿和空间点坐标
    problem.AddResidualBlock(costfunctor, 
                             nullptr, 
                             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::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，其他可选项如下：
- DENSE_QR：QR分解，用于小规模最小二乘问题求解；
- DENSE_NORMAL_CHOLESKY&SPARSE_NORMAL_CHOLESKY：Cholesky分解，用于具有稀疏性的大规模非线性最小二乘问题求解；
- CGNR：使用共轭梯度法求解稀疏方程；
- DENSE_SCHUR&SPARSE_SCHUR：SCHUR分解，用于BA问题求解；
- ITERATIVE_SCHUR：使用共轭梯度SCHUR求解BA问题；
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_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);

minimer_progress_to_stdout

该选型默认为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; // 输出优化过程信息

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";// 输出完整的报告