第十章 位姿与点联合估计(优化版)

Chapter 10   Pose-and-Point Estimation (Optimized)


本章导航 / Chapter Navigator

一句话总结(TL;DR)

束调整(BA)和 SLAM 同时优化相机位姿和路标点位置;利用箭头形稀疏结构Schur 补,可以把求解复杂度从 降到

阅读路径建议:

  • 🟢 快速浏览:读完”直觉图解”即可把握核心思想
  • 🔵 理解算法:阅读”概念层”的公式和解释
  • 深度推导:展开”推导层”查看完整数学

10.1 束调整(Bundle Adjustment):视觉 SLAM 的核心

🎯 直觉:同时解”在哪里拍”和”拍到了什么”

问题:你有 张照片和 个路标点。每张照片看到某些路标点。求:

  1. 每张相机的位姿 (在哪里拍的)
  2. 每个路标点的位置 (拍到了什么)

关键约束:仅靠视觉测量,绝对坐标系无法确定(平移、旋转、尺度不可观)。

处理:固定第 0 个位姿 ,消除规范自由度。


🔑 概念层:优化目标与稀疏结构

状态

测量残差

其中 是相机投影模型。

目标函数

Gauss-Newton 迭代

  1. 在当前点线性化(位姿用左扰动,路标用线性扰动)
  2. 构建并求解
  3. 更新:
  4. 重复直到收敛

🔑 概念层:箭头形稀疏结构

线性系统 的矩阵 具有特殊结构:

关键观察

结构原因
(位姿-位姿)块对角每个测量只关联一个位姿,位姿之间无直接耦合
(路标-路标)块对角每个路标独立,只通过位姿间接耦合
(位姿-路标)稀疏只在有位姿观测到路标时有非零块

整体呈箭头形(arrowhead)


🔑 概念层:Schur 补消元

策略:先消去路标变量,求解缩减的位姿系统,再回代求路标。

Schur 补方程

为什么高效?

  • 块对角,其逆可逐块求( 矩阵求逆)
  • 缩减系统规模为 (而非
  • 复杂度: 对比

回代


🧮 数值示例:简化 BA 问题

场景:2 个位姿,3 个路标

  • 位姿 1 观测路标 1、2
  • 位姿 2 观测路标 2、3

矩阵 结构(示意):

\mathbf{A}_{11}^{(1)} & & \mathbf{A}_{12}^{(1,1)} & \mathbf{A}_{12}^{(1,2)} & \\ & \mathbf{A}_{11}^{(2)} & & \mathbf{A}_{12}^{(2,2)} & \mathbf{A}_{12}^{(2,3)} \\ \hline \mathbf{A}_{12}^{(1,1)T} & & \mathbf{A}_{22}^{(1)} & & \\ \mathbf{A}_{12}^{(1,2)T} & \mathbf{A}_{12}^{(2,2)T} & & \mathbf{A}_{22}^{(2)} & \\ & \mathbf{A}_{12}^{(2,3)T} & & & \mathbf{A}_{22}^{(3)} \end{array}\right]$$ **Schur 补过程**: 1. 计算 $\mathbf{A}_{22}^{-1}$:只需分别求 3 个 $3\times3$ 块的逆 2. 构建缩减位姿矩阵 $\tilde{\mathbf{A}}_{11}$($12\times12$) 3. 求解位姿更新 4. 回代求路标更新 --- ## 10.2 SLAM:加入运动先验 ### 🎯 直觉:BA vs. SLAM | | BA | SLAM | |--|----|------| | **信息来源** | 纯视觉测量 | 视觉测量 + 运动先验 | | **位姿间关系** | 独立 | 通过运动模型耦合 | | **规范自由度** | 需固定 $\mathbf{T}_0$ | 运动先验提供绝对参考 | | **$\mathbf{T}_0$** | 固定不参与优化 | 可参与估计 | **运动先验**:来自 IMU、轮速计或常速假设。 --- ### 🔑 概念层:SLAM 的 MAP 目标 **状态**:$\mathbf{x} = \{\mathbf{T}_0, \mathbf{T}_1, \ldots, \mathbf{T}_K, \mathbf{p}_1, \ldots, \mathbf{p}_M\}$ **运动误差**: $$\mathbf{e}_{v,k} = \begin{cases} \ln(\check{\mathbf{T}}_0\mathbf{T}_0^{-1})^\vee & k=0 \\ \ln(\exp(\Delta t_k\boldsymbol{\varpi}_k^\wedge)\mathbf{T}_{k-1}\mathbf{T}_k^{-1})^\vee & k\geq1 \end{cases}$$ **总代价函数**: $$J(\mathbf{x}) = \underbrace{\sum_k \frac{1}{2}\mathbf{e}_{v,k}^T\mathbf{Q}_k^{-1}\mathbf{e}_{v,k}}_{\text{运动先验}} + \underbrace{\sum_{j,k} \frac{1}{2}\mathbf{e}_{y,jk}^T\mathbf{R}_{jk}^{-1}\mathbf{e}_{y,jk}}_{\text{视觉测量}}$$ --- ### 🔑 概念层:SLAM 的稀疏结构 **关键发现**:引入运动先验**不破坏**箭头形结构! - $\mathbf{A}_{22}$(路标-路标):与 BA 相同,仍块对角 - $\mathbf{A}_{11}$(位姿-位姿):增加运动先验项 $\mathbf{F}^{-T}\mathbf{Q}^{-1}\mathbf{F}^{-1}$,该项是**块三对角的** **两种求解策略**: | 策略 | 利用的稀疏性 | 适用场景 | |------|-------------|---------| | **Schur 补** | $\mathbf{A}_{22}$ 块对角 | 路标数 $M \gg$ 位姿数 $K$ | | **Cholesky** | $\mathbf{A}_{11}$ 块三对角 | 位姿数 $K \gg$ 路标数 $M$ | **因子图视角**: - 每个测量和运动先验是因子(黑点) - 每个位姿和路标是变量节点 - 边的稀疏性 = 矩阵 $\mathbf{A}$ 的稀疏性 --- ### 🧮 数值示例:最小 SLAM 问题 **场景**:3 个位姿($\mathbf{T}_0, \mathbf{T}_1, \mathbf{T}_2$),3 个路标 **代价函数共 9 项**: - 运动先验(3项):$J_{v,0} + J_{v,1} + J_{v,2}$ - 视觉测量(6项):$J_{y,10} + J_{y,30} + J_{y,11} + J_{y,21} + J_{y,22} + J_{y,32}$ **矩阵结构**: - $\mathbf{A}_{11}$:$18\times18$(3 位姿 $\times$ 6 维),块三对角 - $\mathbf{A}_{22}$:$9\times9$(3 路标 $\times$ 3 维),块对角 **与 BA 的区别**: - 运动先验保证 $\mathbf{A}$ 始终良条件(即使无测量也有先验解) - $\mathbf{T}_0$ 参与估计,不固定 --- ## 本章自检清单 阅读完本章,你应该能回答: - [ ] BA 的状态包含什么?($K$ 个位姿 + $M$ 个路标) - [ ] 为什么 BA 需要固定 $\mathbf{T}_0$?(视觉测量无法确定绝对坐标系) - [ ] 矩阵 $\mathbf{A}$ 的箭头形结构是什么?(位姿-位姿块对角,路标-路标块对角) - [ ] Schur 补如何将复杂度从 $O((K+M)^3)$ 降到 $O(K^3 + K^2M)$?(消去路标,求解缩减位姿系统) - [ ] SLAM 与 BA 的本质区别?(SLAM 有运动先验,位姿间耦合) - [ ] 两种利用稀疏性的策略分别适合什么场景?(Schur 补适合 $M \gg K$,Cholesky 适合 $K \gg M$) **如果以上都清楚,你掌握了 BA/SLAM 的核心!** --- ## 延伸阅读与代码 **Python 实现(BA 核心)**: ```python import numpy as np from scipy.sparse import csr_matrix, block_diag from scipy.sparse.linalg import spsolve def bundle_adjustment(poses, landmarks, observations, max_iter=10): """ 简化版束调整 poses: [K, 4, 4] 位姿矩阵 landmarks: [M, 3] 路标位置 observations: [(k, j, y, R)] 观测列表 """ K, M = len(poses), len(landmarks) for iteration in range(max_iter): # 构建线性系统 A x = b # A_11: 位姿-位姿 (6K x 6K) # A_22: 路标-路标 (3M x 3M) # A_12: 位姿-路标 (6K x 3M) A_11_blocks = [np.zeros((6, 6)) for _ in range(K)] A_22_blocks = [np.zeros((3, 3)) for _ in range(M)] A_12_blocks = [[None for _ in range(M)] for _ in range(K)] b_1 = np.zeros(6 * K) b_2 = np.zeros(3 * M) for (k, j, y, R) in observations: # 计算残差和 Jacobian(简化,假设针孔相机) T = poses[k] p = landmarks[j] # 投影到相机坐标系 p_cam = T[:3, :3] @ p + T[:3, 3] y_pred = p_cam[:2] / p_cam[2] # 针孔投影 residual = y - y_pred # Jacobian(简化版本) # G_1: 位姿 Jacobian (2x6) - 使用左扰动 depth = p_cam[2] G_1 = np.zeros((2, 6)) G_1[:, 0] = [1/depth, 0] # x平移 G_1[:, 1] = [0, 1/depth] # y平移 G_1[:, 2] = [-p_cam[0]/depth**2, -p_cam[1]/depth**2] # z平移 G_1[:, 3] = [-p_cam[0]*p_cam[1]/depth**2, -(1+p_cam[1]**2/depth**2)] # 旋转x G_1[:, 4] = [1+p_cam[0]**2/depth**2, p_cam[0]*p_cam[1]/depth**2] # 旋转y G_1[:, 5] = [-p_cam[1]/depth, p_cam[0]/depth] # 旋转z # G_2: 路标 Jacobian (2x3) G_2 = np.zeros((2, 3)) G_2[0, :] = [1/depth, 0, -p_cam[0]/depth**2] @ T[:3, :3] G_2[1, :] = [0, 1/depth, -p_cam[1]/depth**2] @ T[:3, :3] R_inv = np.linalg.inv(R) # 填充 A 矩阵块 A_11_blocks[k] += G_1.T @ R_inv @ G_1 A_22_blocks[j] += G_2.T @ R_inv @ G_2 if A_12_blocks[k][j] is None: A_12_blocks[k][j] = np.zeros((6, 3)) A_12_blocks[k][j] += G_1.T @ R_inv @ G_2 b_1[6*k:6*(k+1)] += G_1.T @ R_inv @ residual b_2[3*j:3*(j+1)] += G_2.T @ R_inv @ residual # 组装块对角矩阵 A_22_inv_blocks = [np.linalg.inv(A_22_blocks[j] + 1e-6*np.eye(3)) for j in range(M)] # Schur 补:构建缩减位姿系统 A_reduced = block_diag(A_11_blocks).toarray() b_reduced = b_1.copy() for k in range(K): for j in range(M): if A_12_blocks[k][j] is not None: A_12 = A_12_blocks[k][j] A_22_inv = A_22_inv_blocks[j] # Schur 补项 A_reduced[6*k:6*(k+1), 6*k:6*(k+1)] -= A_12 @ A_22_inv @ A_12.T b_reduced[6*k:6*(k+1)] -= A_12 @ A_22_inv @ b_2[3*j:3*(j+1)] # 求解位姿更新 delta_xi = np.linalg.solve(A_reduced, b_reduced) # 更新位姿 for k in range(K): delta = delta_xi[6*k:6*(k+1)] # 指数映射更新(简化) poses[k][:3, 3] += delta[:3] # 平移近似 poses[k][:3, :3] = so3_exp(delta[3:6]) @ poses[k][:3, :3] # 回代求路标更新 delta_p = np.zeros(3 * M) for j in range(M): delta_p[3*j:3*(j+1)] = A_22_inv_blocks[j] @ ( b_2[3*j:3*(j+1)] - sum( A_12_blocks[k][j].T @ delta_xi[6*k:6*(k+1)] for k in range(K) if A_12_blocks[k][j] is not None ) ) # 更新路标 landmarks += delta_p.reshape(M, 3) # 检查收敛 if np.linalg.norm(delta_xi) < 1e-6: break return poses, landmarks ``` --- *下一章将引入连续时间高斯过程先验,实现更平滑的轨迹估计。*