【学习随笔】机器人感知-因子图在SLAM中的应用
本文仅用于记录自己学习过程中的总结。
0.
购于2022年9月28，计划十一期间看完一遍。
第1章 引言
贝叶斯网络是一个有向图模型，图中的节点代表变量 θ j \theta_j θj，将一个贝叶斯网络全部变量 Θ \Theta Θ上的联合概率密度函数 p ( Θ ) p(\Theta) p(Θ) 定义为节点的乘积： p ( Θ ) = Π j p ( θ j ∣ π j ) p(\Theta)=\Pi_j p(\theta_j|\pi_j) p(Θ)=Πjp(θj∣πj) 其中 π j \pi_j πj是 θ j \theta_j θj的父节点。
贝叶斯网络是一种非常适用于建模的语言，相比而言因子图则更适用于推断。然而因子图比bayes网络更加通用，因为它不仅可以指定概率密度，还可以指定在变脸 X X X的集合上的任意一个因子函数 ϕ ( X ) \phi(X) ϕ(X)。
与Bayes网络不同，银子图中观测量不会被显示地表示出来
困惑：表示成因子后，如何确定谁是先验、谁是后验？
原始采样（ancestral sampling）只适用于 有向无环图，通常采样是使用马可夫链蒙特卡洛方法，其中之一是吉布斯采样（Gibbs sampling）。
因子图以其能够在最大后验概率推断问题中表示未归一化后验概率的能力，成了概率机器人领域中理想的图模型。
20220929
第2章 平滑与地图构建
平滑与地图构建（smoothing and mapping, SAM）
对于一个符合高斯噪声模型的SLAM问题进行最大后验概率推断，等价于求解一个非线性最小二乘问题。
白化：whitening，相当于消掉了观测量的单位。
求解最小二乘问题 A x = b Ax=b Ax=b：
Cholesky分解： A T A x = A T b A^TAx=A^Tb ATAx=ATb， Λ = A T A \Lambda=A^TA Λ=ATA 称为信息矩阵，通过cholesky分解为三角矩阵 Λ = R T R \Lambda=R^TR Λ=RTR，最终有 R T ( R x ) = A T b R^T(Rx)=A^Tb RT(Rx)=ATb，进而求解；对于 m ∗ n m*n m∗n的矩阵 A A A，计算复杂度为 ( m + n / 3 ) n (m+n/3)n (m+n/3)n。
QR分解：直接对 A A A阵进行分解，利用Householder变换， H n . . . H 2 H 1 A = Q T A = [ R , 0 ] T H_n...H_2H_1A=Q^TA=[R, 0]^T Hn...H2H1A=QTA=[R,0]T，其中 Q Q Q为正交矩阵，此时 ∣ A x − b ∣ 2 2 = ∣ Q T A x − Q T b ∣ 2 2 = ∣ R x − d 2 2 ∣ + ∣ e ∣ 2 2 |Ax-b|_2^2=|Q^TAx-Q^Tb|_2^2=|Rx-d_2^2|+|e|_2^2 ∣Ax−b∣22=∣QTAx−QTb∣22=∣Rx−d22∣+∣e∣22，其中 d = R x d=Rx d=Rx。计算复杂度主要为Householder变化，为 O ( 2 ( m − n / 3 ) n 2 ) O(2(m-n/3)n^2) O(2(m−n/3)n2)。
总结：SLAM优化问题，变为稀疏线性代数的方式表述，这类方法乘坐平方根SAM，简记为 S A M \sqrt {SAM} SAM。
优化方法：
梯度下降：一阶
高斯牛顿 GN：二阶
列文博格-马夸尔特 LM： ( A T A + λ I ) Δ x = A T b (A^TA+\lambda I)\Delta x=A^Tb (ATA+λI)Δx=ATb，介于梯度和GN之间；当接收更新时，降低 λ \lambda λ，拒绝更新时，提高 λ \lambda λ；
DogLeg（PDL）：将梯度下降方向向GN方向弯折，停止在置信区域边界，避免了LM算法拒绝迭代时重新计算分解的计算量。
GN和PDL都要求信息矩阵 Λ \Lambda Λ是可逆的，对于欠约束或病态问题，建议采用LM。
EKF等滤波方法对于SLAM的问题是，线性化选择没有办法撤销；SLAM的平滑问题，在勘测学和摄影测量中称作光束法平差BA，在计算机视觉中称作运动重建SfM。平方根SAM从根本上优于EKF方法，被认为是“应该总是被优先应用于标准的卡尔曼滤波器的递归中”。
参考论文：
Dellaert, F., & Kaess, M. (2006). Square Root SAM: Simultaneous localization and mapping via square root information smoothing. The International Journal of Robotics Research, 25(12), 1181-1203.
20220930
总结
实在懒得抄了，大概10月3把这本书翻完了。整体理解了因子图的思想。但可能还不够深入，没有形成系统。
尤其是因子图与Bayes图有什么关系，具体为什么用xxx。
但今日又重新翻看了一边《十四讲》，将相关内容再次思考，整理如下：
第11讲
SLAM是一个图优化问题，可以自然表示成一个动态Bayes网络。
Bayes网络是一种概率图，由随机变量的节点和表达随即条件独立性的边组成，形成一个有向无环图。SLAM中，运动方程和观测方程，恰好表示状态变量之间的条件概率。
例如，箭头表示了先验后验等，例如 P ( x 1 ∣ x 2 , u 1 ) P(x_1|x_2, u_1) P(x1∣x2,u1)为运动方程，在图上存在一个箭头。
我们发现在计算后验概率最大时，是多个因式相乘，故Bayes网络可以转化成一个因子图。
因子节点包含了待优化变量之间的关系（因子来自运动和观测方程），因子图优化的目的，即调整变量的值，使因子的乘积最大化。
因子图和普通的图优化比，在优化时没有显著区别， 但增量式平滑与建图(incremental smooth and mapping)的iSAM，使其后端可以增量式处理优化，而不是像原始的图优化，每次加入节点后都需要整个图进行优化。
20221005