后端

2024-02-29 15:11:04

一、Bundle Adjustment

1.1 文献阅读

我们在第五讲中已经介绍了 Bundle Adjustment，指明它可以⽤于解 PnP 问题。现在，我们又在后端中说明了它可以⽤于解⼤规模的三维重构问题，但在实时 SLAM 场合往往需要控制规模。事实上，Bundle Adjustment 的历史远⽐我们想象的要长。请阅读 Bill Triggs 的经典论⽂ Bundle Adjustment: A Modern Synthesis（见 paper/⽬录），了解 BA 的发展历史，然后回答下列问题：

为何说 Bundle Adjustment is slow 是不对的？

在Bundle Adjustment时，\(H\Delta x = g\) 中的矩阵H具有稀疏性，可以利用这种稀疏性加速计算。
BA 中有哪些需要注意参数化的地⽅？Pose 和 Point 各有哪些参数化⽅式？有何优缺点。

3D Point :

即使对于校准过的相机，视觉几何和视觉重建在本质上也是投影的。如果一个 \(3D (X,Y, Z)^T\) 参数（或等效的齐次仿射\((X ,Y, Z, 1)^T\)）用于非常遥远的3D点，需要大的\(X,Y,Z\)位移来显著改变图像。即，在(X Y Z)空间中，成本函数变得非常平坦，以及远距离的点的调整成本过大。

相比之下，对于齐次射影参数化\((X,Y,Z,W)^T\)，只要归一化使齐次 4-vector 在无穷远处保持有限(通过\(W→0\))，则其在无穷远处的行为是自然的、有限的和条件良好的。我们需要使用齐次参数化\((X,Y,Z,W)^T\)来处理远处的点，例如球形标准化 \(\Sigma X_i^2=1\) 。

rotation:

经验表明像欧拉角这样的准全局 3 个参数的旋转参数化会引起数值问题，除非能够避免它的奇异性和不均匀覆盖的范围。

旋转应该参数化使用四元数服从 \(||q||^2=1\) ，或通过局部扰动 \(RδR\) 或 \(δRR\) 现有的旋转R,其中 \(δR\) 可以是任何良好性能的 3参数小量的旋转近似。例如\(δR = (I + [ δr]_×)\) ，罗德里格斯公式，局部欧拉角。
*本⽂写于 2000 年，但是⽂中提到的很多内容在后⾯⼗⼏年的研究中得到了印证。你能看到哪些⽅向在后续⼯作中有所体现？请举例说明。

Bundle Adjustment 成为视觉SLAM后端主流优化方法。

1.2 BAL-dataset

BAL（Bundle Adjustment in large）数据集是⼀个⼤型 BA 数据集，它提供了相机与点初始值与观测，你可以⽤它们进⾏ Bundle Adjustment。现在，请你使⽤ g2o，⾃⼰定义 Vertex 和 Edge（不要使⽤⾃带的顶点类型，也不要像本书例程那边调⽤ Ceres 来求导），书写 BAL 上的 BA 程序。你可以挑选其中⼀个数据，运⾏你的 BA，并给出优化后的点云图。

本题不提供代码框架，请独⽴完成。提⽰：

注意 BAL 的投影模型⽐教材中介绍的多了个负号；

le/build$ ./g2o_customBundle -input ../data/problem-16-22106-pre.txt 
Header: 16 22106 83718bal problem file loaded...
bal problem have 16 cameras and 22106 points. 
Forming 83718 observatoins. 
beginning problem...
Normalization complete...
begin optimizaiton ..
iteration= 0	 chi2= 8371319.036499	 time= 0.401498	 cumTime= 0.401498	 edges= 83718	 schur= 1	 lambda= 954109472863025471952004134744517163213937180672.000000	 levenbergIter= 1
optimization complete..

二、直接法的 Bundle Adjustment

2.1 数学模型

特征点法的 BA 以最⼩化重投影误差作为优化⽬标。相对的，如果我们以最⼩化光度误差为⽬标，就得到了直接法的 BA。之前我们在直接法 VO 中，谈到了如何⽤直接法去估计相机位姿。但是直接法亦可⽤来处理整个 Bundle Adjustment。下⾯，请你推导直接法 BA 的数学模型，并完成它的 g2o 实现。注意本题使⽤的参数化形式与实际的直接法还有⼀点不同，我们⽤ x,y,z 参数化每⼀个 3D 点，⽽实际的直接法多采⽤逆深度参数化 [1]。

本题给定 7 张图⽚，记为 0.png ⾄ 6.png，每张图⽚对应的相机位姿初始值为 \(T_i\)，以 \(T_{cw}\) 形式存储在 poses.txt ⽂件中，其中每⼀⾏代表⼀个相机的位姿，格式如之前作业那样：

\(time,t_x,t_y,t_z,q_x,q_y,q_z,q_w\)

平移在前，旋转（四元数形式）在后。同时，还存在⼀个 3D 点集 P，共 N 个点。其中每⼀个点的初始坐标记作 \(p_i = [x,y,z]^T_i\) 。每个点还有⾃⼰的固定灰度值，我们⽤ 16 个数来描述，这 16 个数为该点周围 4x4 的⼩块读数，记作 \(I(p)_i\)，顺序见图 1 。换句话说，⼩块从 u−2,v −2 取到 u + 1,v + 1，先迭代 v。那么，我们知道，可以把每个点投影到每个图像中，然后再看投影后点周围⼩块与原始的 4x4 ⼩块有多⼤差异。那么，整体优化⽬标函数为：

\(\min \sum^{7}_{j=1}\sum^{N}_{i=1}\sum_{W}||I(p_i)-I_j(\pi(KT_j\:p_i))||^2_2\)

即最⼩化任意点在任意图像中投影与其本⾝颜⾊之差。其中 K 为相机内参（在程序内以全局变量形式给定），π 为投影函数，W 指代整个 patch。下⾯，请回答：

如何描述任意⼀点投影在任意⼀图像中形成的 error？

通过相邻帧的像素差 \(\begin{equation} e = {\mathbf{I}_1}\left( {{\mathbf{p}_1}} \right) - {\mathbf{I}_2}\left( {{\mathbf{p}_2}} \right) \end{equation}\)
每个 error 关联⼏个优化变量？

路标点point和相机位姿pose
error 关于各变量的雅可⽐是什么？

\(\frac{{\partial \mathbf{u}}}{{\partial \delta \mathbf{\xi} }} = \left[ {\begin{array}{*{20}{c}} {\frac{{{f_x}}}{Z}}&0&{ - \frac{{{f_x}X}}{{{Z^2}}}}&{ - \frac{{{f_x}XY}}{{{Z^2}}}}&{{f_x} + \frac{{{f_x}{X^2}}}{{{Z^2}}}}&{ - \frac{{{f_x}Y}}{Z}}\ 0&{\frac{{{f_y}}}{Z}}&{ - \frac{{{f_y}Y}}{{{Z^2}}}}&{ - {f_y} - \frac{{{f_y}{Y^2}}}{{{Z^2}}}}&{\frac{{{f_y}XY}}{{{Z^2}}}}&{\frac{{{f_y}X}}{Z}} \end{array}} \right]\)

\(\mathbf{J} = - \frac{{\partial { \mathbf{I}_2}}}{{\partial \mathbf{u}}}\frac{{\partial \mathbf{u}}}{{\partial \delta \mathbf{\xi} }}\)

2.2 实现

下⾯，请你根据上述说明，使⽤ g2o 实现上述优化，并⽤ pangolin 绘制优化结果。程序框架见 code/directBA.cpp ⽂件。实现过程中，思考并回答以下问题：

能否不要以 \([x,y,z]^T\) 的形式参数化每个点？
取 4x4 的 patch 好吗？取更⼤的 patch 好还是取⼩⼀点的 patch 好？

可以，可以增大patch，这会减少误差，
从本题中，你看到直接法与特征点法在 BA 阶段有何不同？

误差函数不同，直接法是像素差最小，而特征点法是重投影误差最小。
由于图像的差异，你可能需要鲁棒核函数，例如 Huber。此时 Huber 的阈值如何选取？

\(H(e)=\left\{\begin{matrix} \frac{1}{2}e^2,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\, 当|e| \leqslant \delta\\ \delta (|e|-\frac{1}{2}\delta),\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,其它 \end{matrix}\right.\)

提⽰：

构建 Error 之前先要判断点是否在图像中，去除⼀部分边界的点。
优化之后，Pangolin 绘制的轨迹与地图如图 1 所⽰。
你也可以不提供雅可⽐的计算过程，让 g2o ⾃⼰计算⼀个数值雅可⽐。
以上数据实际取⾃ DSO[1]。

图 1: 直接法 BA 的图例。左上：点颜⾊的定义顺序，其中 11 号点是观测到的位置；右上：图⽚⽰例；中间：优化后的相机位置与点云。

码农公寓