后端
一、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 号点是观测到的位置;右上:图⽚⽰例;中 间:优化后的相机位置与点云。