优化

namespace: optimization

在回环检测中提到了，tracking一定会有累积误差，需要闭环检测提供约束来优化消除误差，优化也就是这部分的内容。

后端优化一般会用一个优化器，g2o，ceres都是很不错的，可惜的是作者只会Eigen。当然，对于优化部分自己用Eigen实现，应该会更有助于学习吧。OnePiece实现了两种后端优化方式：Fast BundleAdjustment与BundleAdjustment。

Fast BundleAdjustment

介绍之前，先介绍一个数据结构：Correspondence:

class Correspondence
{
    public:
    int source_id = -1;
    int target_id = -1;
    float average_disparity = 1e6;
    geometry::PointCorrespondenceSet correspondence_set;
};

一个Frame Correspondence意思是一对匹配的frame，其中source id是source frame在优化变量中的id，target id是target frame在优化变量中的id。而average_disparity用来衡量这些配对点在2维图片上的位移，主要是根据这个位移来判断是否应该开始一个新的关键帧。geometry::PointCorrespondenceSet是这一对匹配的frame包含的匹配点的集合。

Fast BundleAdjustment（FBA）比传统的BundleAdjustment更快。实际上FBA是在OnePiece中的叫法，并没有一个很标准的名字。它和BundleAdjustment的区别在于优化变量与目标函数。FBA只优化相机位姿，优化目标函数为：

\[ E(T) = \sum_{i}^{\vert\Omega\vert}\sum_{j}^{\vert C_{i}\vert}\Vert T_sp_{s(j)} - T_tp_{t(j)} \Vert^2 \]

\(\Omega\)为所有的frame correspondences，\(C_{i}\)指的是第\(i\)个frame correspondence，由多个point correspondence组成。\(T_s, T_t\)分别代表了frame correspondence中的source frame的位姿与target frame的位姿，而\(p_{s(j)}\)表示第\(j\)个point correspondence中的source point。对于这部分的优化，可以参考文章fastgo，也就是related_paper/fastgo.pdf。在这篇文章中还包含了有更多的优化技巧，提高系统效率，而OnePiece只实现了比较基础的部分，函数如下：

void FastBA(const std::vector<Correspondence> &correspondences, geometry::SE3List &poses, int max_iteration =5);

BundleAdjutment

传统的BundleAdjustment（BA）除了优化位姿，还会优化世界坐标系下的三维点坐标，同时优化目标是二维的重投影误差。一般来说，传统BA的输入是世界坐标系下3d点，相机位姿，3D点在各个相机位姿下的投影2D坐标。目标函数为：

\[ E(T, p) = \sum_i^{N_f}\sum_{j}^{N_p}\Vert u_{i,j} - \omega(T_i^{-1} p_j) \Vert, \]

\(N_f\)是相机位姿（frame）的数量，\(N_p\)是世界坐标点的数量，而\(u_{i,j}\)是实际投影的位置（特征匹配时候的uv坐标），而\(\omega\)是计算投影的函数，将世界点根据相机位姿重新投影得到的坐标。

对于这种优化，除了优化相机位姿，还有优化了世界下点的位置，这也就优化了投影过程中的噪声。但是，由于世界点的数量往往非常多，使得最后优化矩阵维度上万甚至百万。因此到了后面它的优化速度变得很慢。但是BA是很经典的方法，早在SLAM出现之前就提出了，也有很多它的变种，加速方法，这个只是用来了解一下最基本的BA罢了。

void BundleAdjustment(geometry::Point3List &world_points, 
    std::vector<ProjectedPointsOnFrame> &projected_points, 
    geometry::SE3List &poses,
    const camera::PinholeCamera &camera, int max_iteration = 20);

在实现FastBA时，OnePiece使用高斯牛顿优化法，而BA一定要使用列文伯格-马夸尔特算法，因为很容易出现奇异矩阵，导致高斯牛顿变得很脆弱，使得结果无法收敛。对于BA的详细推导，可以看related_paper/BARevisited.pdf。

点击查看示例。