05 多视图几何视觉 SLAM 3D 视觉机器人

Multi-view Geometry & Visual SLAM

从两条射线到
相机、地图与物体

单幅图像把深度压缩进一条射线；跨帧对应把多条射线联系起来，才能约束相机运动并三角化场景。视觉 SLAM 再把这些局部约束组织成可持续更新、可重定位并保持全局一致的状态图。

Problem Contract

先固定 pose 方向，才谈得上两视图约束

本站继续使用右手系、列向量和 target-from-source 记号。相机 pose 写作 world-from-camera；真正投影世界点时使用它的逆 camera-from-world。相对变换必须通过同一个复合链推导，不能凭变量名猜方向。

World poses → relative camera motion

{}^{C_2}\mathbf T_{C_1}=\left({}^{W}\mathbf T_{C_2}\right)^{-1}{}^{W}\mathbf T_{C_1},\qquad {}^{C_2}\mathbf p=\mathbf R_{21}\,{}^{C_1}\mathbf p+\mathbf t_{21}

R21、t21 把 C1 系中的三维坐标变到 C2 系。t21 是“C1 原点在 C2 系中的坐标”，不等于世界系中的 C2 相机中心。实验室与下文全部使用这个方向。

像素 u₁

先去畸变，再由 K₁⁻¹ 得到 bearing x₁

↔

MATCH

同一物理点

特征、光度、光流或稠密对应提供身份关系

↔

像素 u₂

对应 bearing x₂ 必须使用自身 K₂ 与畸变状态

→

GEOMETRY

motion + structure

模型选择、relative pose、cheirality 与 triangulation

对应不是几何定理自动产生的。 极线、PnP 和 BA 都是在“对应身份大体正确”的条件下解释观测。描述子匹配、tracking、语义关联和 object identity 是数据关联层；RANSAC 能抵抗一部分离群，但不能把完全错误的身份恢复成真。

Epipolar Geometry

E 与 F 把二维搜索压缩到一条极线

同一三维点、两个相机中心和两条观测射线共面，因此第二幅图中的正确对应落在由第一幅图点决定的极线上。Essential matrix 绑定已标定相机的相对刚体运动；Fundamental matrix 把内参也吸收到像素域关系中。

Calibrated epipolar constraint

\mathbf x_2^\mathsf T\mathbf E\mathbf x_1=0,\qquad \mathbf E=[\mathbf t_{21}]_\times\mathbf R_{21},\qquad \operatorname{sv}(\mathbf E)=(\sigma,\sigma,0)

E 只有五个自由度：相对旋转 3，加单位平移方向 2。把 t21 乘任意非零尺度，约束仍成立；因此 E 本身不携带单目 baseline 长度。

Pixel-domain fundamental matrix

\mathbf u_2^\mathsf T\mathbf F\mathbf u_1=0,\qquad \mathbf F=\mathbf K_2^{-\mathsf T}\mathbf E\mathbf K_1^{-1},\qquad \operatorname{rank}(\mathbf F)=2

若点已去畸变到新的 camera matrix，必须用与这些点匹配的 K。直接把畸变像素塞入理想 pinhole 的 F/E 约束，会得到位置相关的系统残差。

模型	输入坐标	自由度	最小对应	适用	首要退化
Essential matrix · E	已标定归一化坐标 x₁ ↔ x₂	5 DoF：R(3) + t direction(2)	5-point；也常用 normalized 8-point	一般三维场景中的 calibrated relative pose	纯旋转时 E=0；低视差深度病态；平面场景存在弱可辨识性
Fundamental matrix · F	原始像素 u₁ ↔ u₂	rank-2 projective matrix，整体尺度无意义	7-point / 8-point	未知或尚未使用内参的两视图极线几何	不能直接当作 metric pose；坐标预处理与数值归一化非常关键
Homography · H	同一平面像素或纯旋转下的全图像素	8 DoF，整体尺度无意义	4 对非退化点	平面、纯旋转、图像拼接与退化分支	一般三维场景中不同深度不能被同一个 H 正确解释

ALGEBRAIC

|x₂ᵀEx₁|

便于写公式，但数值随 E 尺度与坐标缩放改变，不适合作为可比较的几何距离。

SAMPSON

一阶几何误差近似

用 E x₁ 与 Eᵀx₂ 的前两维归一化代数残差；RANSAC 中常用，但仍不是完整非线性重投影误差。

REPROJECTION

把三维假设投回两幅图

有了 pose 与三维点后，联合两视图或多视图像素残差最接近最终 BA 目标。

Model Selection & Degeneracy

先问场景支持哪个模型，再从模型恢复 pose

五点、八点和 Homography 不是互相替代的“算法口味”，而是不同几何假设。一般三维场景由 E/F 约束；单一平面和纯旋转可被 H 解释。可靠初始化会同时比较支持度、残差、视差和 cheirality。

Plane-induced homography · nᵀX=d convention

\mathbf H=\mathbf K_2\!\left(\mathbf R_{21}+\frac{\mathbf t_{21}\mathbf n^\mathsf T}{d}\right)\!\mathbf K_1^{-1},\qquad \mathbf n^\mathsf T\mathbf X=d

符号取决于平面方程约定。这里 n 与 X 都在 C1 frame；若 t21=0，则所有深度共享 pure-rotation homography H=K2 R21 K1⁻¹。不要从记忆中抄一个符号而不写 plane convention。

RANSAC 解决离群，不解决不可观。 即使抽到了全部正确对应，纯旋转仍没有三角化基线，平面仍存在弱分支，低视差仍会放大深度方差。应把 model score 与 observability check 分开实现和记录。

Triangulation

极线约束检验共面，三角化才把射线变成深度

理想无噪声时两条世界射线相交；真实观测中它们通常异面，因此需要最小化代数、射线距离或重投影误差。所有方法都无法绕过几何条件数：视差趋近零时，深度不确定性急剧增长。

DLT

线性齐次初始化

由多视图投影方程构造 A X=0 并取最小奇异向量；快速，但代数目标不等同于像素噪声模型。

MIDPOINT

重投影精修

以线性解为初值，在两个或更多视图中最小化 robust reprojection error，并检查正深度。

INVERSE DEPTH

远点与延迟初始化

用 ρ=1/Z 表示深度，远点接近 ρ=0 时数值更平滑；证据不足时不应强行生成 metric point。

Parallax and conditioning intuition

\theta=\cos^{-1}\!\left(\hat{\mathbf d}_1^\mathsf T\hat{\mathbf d}_2\right),\qquad \text{depth sensitivity}\ \propto\ \frac{1}{\sin\theta}

这不是完整协方差公式，但准确表达了退化趋势：θ→0 时两射线近乎平行，极小像素扰动可造成极大深度变化。阈值应结合焦距、噪声、baseline 与任务深度，而非迷信固定角度。

Interactive Geometry Lab

同一组约定，贯通两视图、退化与 SLAM 图

第一个视图实际计算 E、Sampson 距离、射线最近点三角化、视差和正深度；第二个视图在四种受控几何中比较 ground-truth E 与 H 残差；第三个视图改变传感器、物体状态和回环，观察 gauge、状态维数、Schur 主系统和漂移。

真实点 P 三角化 P̂ 观测射线 noisy ray 俯视图显示 X–Z；计算使用完整三维坐标

Epipolar

x₂ᵀEx₁

—Sampson —

Parallax

两条世界射线夹角

——

Triangulation

估计三维点

——

Cheirality

两相机正深度

——

Translation

从 E 可恢复什么？

direction only—

Essential matrix · normalized scale

E / ‖t₂₁‖

x₂ᵀEx₁ = 0

Essential matrix 不携带单目基线长度。 把 t₂₁ 乘任意非零尺度，极线约束仍然成立；因此 calibrated monocular 两视图只能恢复旋转和单位平移方向。三角化深度与整张地图会随同一尺度一起变化。

Visual Odometry → SLAM

SLAM 不是把两视图公式循环调用很多次

持续系统需要管理 keyframe、landmark 生命周期、局部窗口、重定位、回环与地图融合。视觉里程计主要估计连续局部运动；SfM 通常离线恢复相机和结构；SLAM 则在在线定位同时维护可复用且全局一致的地图。

01

前端追踪

匹配 feature、直接最小化光度误差，或使用 learned correspondence / flow；输出带不确定性的跨帧观测。

→

02

初始化

在 E 与 H 等模型之间选择，恢复相对运动、做 cheirality，并只三角化有足够视差的点。

→

03

关键帧与局部地图

用运动、重叠和跟踪质量决定何时插入 keyframe；新点由多视角几何初始化。

→

04

局部优化

联合优化最近相机、landmark 与可选物体状态；鲁棒核、外点剔除和边缘化共同控制窗口。

→

05

重定位

跟踪失败后，用地点识别取得候选，再通过 2D–3D PnP / geometric verification 恢复相机 pose。

→

06

回环与地图融合

验证非局部相遇，优化 pose graph / essential graph，必要时做全局 BA 与 map merge。

前端家族	主要观测 / 残差	优势	风险
Feature-based	关键点、描述子、匹配与重投影残差	对曝光变化和较大基线通常更稳；图结构清晰	低纹理、重复纹理和匹配离散化
Direct / semi-direct	像素强度、patch 或梯度的光度残差	利用更多像素，可在弱纹理下保留连续信号	依赖亮度一致性、良好初始化与曝光/响应建模
Dense / learned	深度、光流、对应场或 neural map residual	可结合语义与强先验，覆盖传统特征困难区域	域偏移、置信度标定、几何一致性和计算成本

VISUAL ODOMETRY

局部增量与短期一致

允许长期漂移；常服务于控制、初始化或 SLAM 前端。

STRUCTURE FROM MOTION

离线全局重建

可多轮匹配、全局 BA 和增量/全局重建，不一定满足实时定位约束。

VISUAL SLAM

在线定位 + 可复用地图

需要重定位、回环、地图维护和失败恢复；“有轨迹输出”并不自动等于 SLAM。

Bundle Adjustment

BA 联合解释所有相机与三维点，而不是逐帧修补 pose

相机 pose、landmark、内参以及可选物体状态共同影响重投影。BA 在流形上迭代这些变量，利用块稀疏 Jacobian 与 Schur complement 消元大量点变量；鲁棒核只降低外点影响，数据关联和 gauge 仍需单独处理。

Robust bundle-adjustment objective

\min_{\{\mathbf T_{C_iW}\},\,\{\mathbf P_j\}}\ \sum_{(i,j)\in\mathcal O}\rho\!\left(\left\|\mathbf u_{ij}-\pi\!\left(\mathbf K_i,\mathbf T_{C_iW},\mathbf P_j\right)\right\|_{\boldsymbol\Sigma_{ij}^{-1}}^2\right)

O 是可见观测集合；Σ 对像素噪声和置信度白化，ρ 是 robust loss。pose 必须通过 SE(3) retraction 更新。局部 BA、滑窗优化和全局 BA 的公式相同，变量范围与 prior 管理不同。

BLOCK SPARSITY

每条观测只连接一台相机与一个点

Jacobian 极稀疏；忽略结构直接构造稠密 normal equation 会浪费数量级资源。

SCHUR

先消元 landmark 块

把主系统缩到相机、内参与物体变量；再回代点增量。

GAUGE

先固定不可观自由度

未锚定系统的 Hessian 奇异；首 pose、metric scale 或合适 prior 必须与传感器可观性一致。

MARGINALIZATION

把旧状态压成 prior

滑窗实时化的代价是线性化点和信息固化；外点若进入 prior，会持续污染后续估计。

回环因子与 gauge 是两件事。 回环约束首尾相对关系，能够分配累计漂移；它不会凭空提供单目 metric scale。单目子地图融合常在 Sim(3) 上进行，因为不同子图可能携带不同尺度。

Scale & Observability

同一套 SLAM 图，传感器决定哪些自由度真正可观

“是否能优化”与“是否能观测”必须区分。优化器可以给任何变量一个数值，但没有独立信息时会存在 gauge、强相关或极大方差。固定变量只选择参考系，不等于创造物理观测。

模态	直接观测	尺度结论	对齐 / 工程提醒
Monocular	bearing / projective geometry	全局尺度不可观；地图与平移只在同一未知比例下成立	评测常需 Sim(3) 对齐；仅固定首帧不能去掉 scale gauge
Calibrated stereo	左右像素 + 已知 baseline	基线把视差变成度量深度	正确标定下使用 SE(3)；baseline 误差会直接变成尺度偏差
RGB-D	彩色像素 + depth / point	有效深度值提供度量尺度	需处理 depth scale、无效值、RGB–Depth 外参与时间同步
Visual–Inertial	视觉 bearing + IMU specific force / angular rate	在充分运动激励和正确初始化下可恢复尺度与重力方向	静止或特定运动会弱可观；还需时间偏移、bias 与外参标定

Gauge-equivalent monocular reconstruction

\mathbf P_j' = s\,\mathbf P_j,\qquad \mathbf t_i'=s\,\mathbf t_i,\qquad \pi(\mathbf R_i\mathbf P_j+\mathbf t_i)=\pi(\mathbf R_i\mathbf P_j'+\mathbf t_i')

对任意 s>0，透视除法后的像素相同。旋转不随尺度变化；相机平移、三维点和物体位置必须一起缩放。known-size object、已知相机高度或轮速也可作为尺度先验，但误差会直接传入地图。

Object-level & Dynamic SLAM

6D 物体位姿既可以是输出，也可以成为地图状态

点地图只保存无语义三维位置；物体级地图显式保存 object frame、类别、尺寸、对称性与不确定性。静态物体可作为大尺度 landmark 帮助相机定位；动态物体则需要每时刻状态、运动模型和跨帧身份关联。

Static object observation factor

{}^{C_i}\mathbf T_{O_j}={}^{C_i}\mathbf T_W\,{}^{W}\mathbf T_{O_j},\qquad \mathbf r_{ij}=\operatorname{Log}\!\left(\widehat{\mathbf T}_{C_iO_j}^{-1}\,{}^{C_i}\mathbf T_W\,{}^{W}\mathbf T_{O_j}\right)

检测器或 6D pose estimator 给出相机到物体的观测；SLAM 图联合优化 camera pose 与 world object pose。残差左右方向、Log 顺序、对称等价类和协方差 frame 必须一起定义。

Dynamic object state

\mathcal X=\left\{{}^{W}\mathbf T_{C_i},\ {}^{W}\mathbf T_{O_j,i},\ \mathbf P_k,\ \text{identity}_j\right\},\qquad {}^{W}\mathbf T_{O_j,i+1}\approx f\!\left({}^{W}\mathbf T_{O_j,i},\mathbf v_{j,i}\right)

动态场景中像素运动是相机运动、物体运动和深度的叠加。若没有静态背景、独立深度或运动先验，两者会强耦合；错误 data association 会把两个物体的轨迹硬连在一起。

REJECT DYNAMIC

把动态像素当外点

适合相机定位优先、静态背景足够的系统；被剔除物体本身没有可用轨迹。

STATIC OBJECT MAP

物体是持久 landmark

实例识别与 6D pose 提供强语义约束，但需处理重复实例、对称和模型误差。

DYNAMIC OBJECT SLAM

相机与物体联合运动

需要 per-object trajectory、运动先验、遮挡生命周期和概率数据关联，状态规模显著增大。

3D VISION

多视图提供 pose、深度、重建与跨帧对应。

VISUAL SLAM

把局部几何组织成相机轨迹和可复用地图。

ROBOTICS

将 camera pose 与 base、hand-eye、tool frame 连接。

GRASPING

持续更新 object pose、速度与不确定性，支撑预测性抓取和闭环操作。

Evaluation & Debugging

先区分数据关联、几何退化、标定和图优化，再看一个总分

ATE、RPE 和重投影误差回答不同问题。单目轨迹若用 Sim(3) 对齐，尺度误差已被吸收；stereo / RGB-D 若也随意做 Sim(3) 对齐，会掩盖系统本应负责的 metric scale。任何轨迹数字都应同时报告对齐方式、坐标 frame、时间匹配与跟踪成功率。

ATE

全局轨迹误差

评估整体位置一致性；对齐可选 SE(3) 或 Sim(3)，必须明确。

RPE

固定时间 / 距离间隔的相对误差

更直接反映局部 drift，但不能替代回环后的全局地图质量。

TRACKING

成功率与恢复能力

报告 lost frames、重定位时间、初始化成功率，而不是只统计成功片段。

MAP / OBJECT

重建、物体 pose 与身份

还需覆盖完整度、表面误差、对称感知 pose 误差和 association 指标。

失败模式	为什么坏	应对 / 验证
纯旋转 / 零基线	E 退化为零，无法恢复 translation direction 或 depth	走 rotation homography；等待发生平移后再建点
单平面或近似平面	H 与 E 均可低残差，初始化分支歧义	双模型竞争；保留多解，等非平面点和新视角验证
低视差 / 远点	极线残差很小，但三角化方差巨大	延迟建点、inverse depth、扩大 keyframe baseline
错误 K / 畸变状态	极线呈系统弯曲或相对 pose 有稳定偏差	先用静态多视图检查标定；保持坐标与 K/D 同步
rolling shutter / 时间不同步	同一帧不存在单一刚体相机 pose	使用 RS / continuous-time 模型，校准相机与 IMU/depth 时间
运动模糊 / 低纹理	对应稀少、光度梯度弱、跟踪突然丢失	曝光与运动策略、更多模态、质量门控和可靠重定位
重复纹理 / 感知混淆	错误匹配或错误回环也能形成较大共识集	局部几何 + 时序 + PnP/Sim(3) 验证；回环采用严格门控
动态物体占主导	前景运动被解释为相机运动，静态世界假设破裂	语义/运动分割、静态背景估计，或显式联合优化 object tracks
单目尺度漂移	不同子地图的尺度不一致，回环无法只用 SE(3) 接上	使用 Sim(3) 回环/地图融合，或引入度量观测
错误 frame / 变换方向	轨迹看似平滑但物体、地图或机器人链整体反向	冻结 world-from-camera 约定，逐项验证复合与求逆
错误回环	一次强约束可把整张地图拉坏	地点检索只是 proposal；几何验证、switchable constraint 与回滚策略
边缘化与外点污染	旧错误被固化进 prior，之后难以恢复	边缘化前清理外点，监控 prior 一致性，必要时重建局部地图

01
冻结坐标与相机模型
打印每帧 ^WT_C、K/D、图像尺寸、时间戳和单位；用真值点检查双视图重投影。
02
先看对应，再看 pose
绘制 match tracks、极线距离、空间覆盖和 track lifetime；错误身份不是换求解器能修好的。
03
分开记录 E、H 与 observability
同时报告两模型支持、median parallax、cheirality、三角化角与深度方差。
04
关闭回环和物体状态
先让最小 VO / local BA 稳定，再逐层加入 loop、IMU、object factors，定位是哪类因子破坏图。
05
检查 BA 每类残差
按帧、landmark、图像区域、动态标签和传感器拆分，不让平均值隐藏结构化偏差。
06
用可恢复故障测试系统
主动注入模糊、遮挡、错误回环和短时丢失，验证质量门控、重定位与地图回滚。

经典教材、原始论文与官方接口展开查看多视图几何、BA、视觉 SLAM 与 Object SLAM 来源