经过前端(视觉里程计)估计得到的轨迹和地图由于存在累计误差,在长时间内是不准确的。因此希望构建一个针对全局的更大规模的优化问题,得到最优的轨迹和地图,这里主要有两种解决思路:
(1)基于马尔可夫性假设的卡尔曼滤波器:马氏性假设可以简单地理解为“当前时刻状态只与上一时刻有关”。针对SLAM问题(非线性)的卡尔曼滤波器给出了单次线性近似下的最大后验估计,或者说是优化过程中一次迭代的结果。
(2)非线性优化:非线性优化考虑了当前及之前所有时刻的状态,同时优化所有时刻的相机位姿以及各个特征点的空间位置。使用光束法平差(BA)求得最优解。
一般情况下认为非线性优化算法比滤波器占有明显优势,因此重点关注BA求解与图优化。
1.问题分析
首先针对全局的BA本质上仍是一个最小化重投影误差的问题:结合任意时刻的相机外部参数(位姿)、内部参数及拍摄到的特征点的三维坐标,计算其投影到图像像素坐标系下的坐标,最小化与实际提取结果之间的误差。所涉及的变量均为前端估计得到的,在这个过程中同时调整相机参数及路标(特征点)坐标。
确定优化问题后,一般我们可以使用常见的优化算法直接求解,计算雅克比矩阵和海塞矩阵求解增量方程,但是由于考虑了过去的所有状态和路标,因此方程的维度极大,很难满足实时性,求解过程中需要进行优化。这里考虑到H矩阵的稀疏性,利用Schur消元(也可以称为边缘化)显著降低计算量,加速运算。不过在实际应用中Ceres和g2o库都可以帮助我们完成这一过程。
最后为削弱异常数据对整个优化过程的影响,提高鲁棒性,提出来几种常用的鲁棒核函数,确保每条边的误差不会大的没边而掩盖其他的边,使优化结果更稳健。
2.示例代码分析
首先是数据读取,这里选择创建了一个BALProblem类,来管理读取到的数据。
/// 从文件读入BAL dataset
class BALProblem {
public:
/// 加载文件数据
explicit BALProblem(const std::string &filename, bool use_quaternions = false);
~BALProblem() {
delete[] point_index_;
delete[] camera_index_;
delete[] observations_;
delete[] parameters_;
}
/// 存储数据到.txt
void WriteToFile(const std::string &filename) const;
/// 存储点云文件
void WriteToPLYFile(const std::string &filename) const;
/// 归一化
void Normalize();