3.3.1 多视角几何与相机模型基本概念
从这一节开始，我们正式从“看一张图像”升级到“理解三维世界”。具身机器人要在真实空间中抓、放、躲、走，都离不开对 3D 几何的建模，而这一切的入口就是相机模型和多视角几何。
3.3.1.1 针孔相机模型
1）从透视投影开始
理想化的相机可以看作一个“针孔”：所有从场景出发的光线都通过同一个小孔，投到后面的成像平面上。这就是经典的 针孔相机模型。
在相机坐标系下，设三维点为
\[ P=(X, Y, Z)^\top \]
针孔相机的基本透视关系可以写成：
\[ x = f \frac{X}{Z},\quad y = f \frac{Y}{Z} \]
其中 \(\displaystyle f\) 是焦距，\(\displaystyle (x,y)\) 是该点在成像平面的坐标（以相机光轴与成像平面的交点为原点）。(cs.cmu.edu)
【图 3-1 占位：针孔相机几何示意图】 图中画出相机坐标系、针孔位置、成像平面，以及从 3D 点 \(\displaystyle P\) 射向针孔并投影到平面的光线，标出焦距 \(\displaystyle f\)、坐标 \((X,Y,Z)\) 与投影点 \(\displaystyle (x,y)\) 的关系。
2）从“理想平面”到真实像素：相机内参矩阵
真实图像的坐标是以左上角为原点的像素坐标 \(\displaystyle (u,v)\)，与上面的理想几何坐标 \(\displaystyle (x,y)\) 还差一个线性变换和偏移。常用 相机内参矩阵 \(\displaystyle K\) 将两者联系起来：
\[ K = \begin{bmatrix} f_x & s & c_x\ 0 & f_y & c_y\ 0 & 0 & 1 \end{bmatrix} \]
其中：
- \((f_x, f_y)\)：x/y 方向的“有效焦距”（包含像素尺寸的缩放）；
- \((c_x, c_y)\)：主点（principal point），即光轴在图像上的交点；
- \(\displaystyle s\)：像素轴之间的切变（大多数相机可近似为 0）。
在齐次坐标下，三维点 \(X=(X,Y,Z,1)^\top\) 到图像点 \(x=(u,v,1)^\top\) 的投影可写成矩阵形式：
\[ x \sim K [R\mid t] X \]
这里：
- \([R\mid t]\) 是相机相对于世界坐标的旋转矩阵和平移向量（外参）；
- “\(\sim\)” 表示相差一个非零尺度因子。(users.cecs.anu.edu.au)
【图 3-2 占位：相机坐标系、世界坐标系及内参与外参示意图】 图中画出世界坐标系、相机坐标系，标出相机位姿 \(([R|t])\)，以及内参矩阵 \(\displaystyle K\) 将相机坐标投影到图像像素的流程框图。
3）畸变一笔带过
真实镜头往往存在 径向畸变（直线变弯）和 切向畸变。在多视几何理论中常假设图像已经做了去畸变，便于使用简单的针孔模型。具体如何标定和去畸变会在 3.4 节详细讨论，这里只需记住：
绝大多数视觉几何公式，都默认你已经把像素坐标“拉直”成理想针孔相机的坐标了。
3.3.1.2 多视角几何
单张图像只能看到“投影”，距离信息被压缩到 2D 中。多视角几何研究的是：当我们有两张或多张从不同视点拍摄的图像时，如何恢复场景的三维结构和相机之间的相对关系 。这是立体视觉、结构光重建、SLAM 等技术的理论基础。(r-5.org)
1）极几何与基础矩阵
考虑两个针孔相机，拍到同一个三维点 \(\displaystyle X\)。在两张图像中的投影分别是 \(\displaystyle x\) 和 \(x’\)。两张图像之间存在一个几何约束，叫做 极线约束：
\[ x’^{\top} F x = 0 \]
这里 \(\displaystyle F\) 是一个 \(3\times3\) 的 基础矩阵（Fundamental Matrix）。它编码了两台 未标定 相机之间的全部极几何信息。(维基百科)
几何含义是：
- 给定左图上的一点 \(\displaystyle x\)，右图中与其对应的点 \(x’\) 必须落在一条 极线 上：\(l’ = F x\)；
- 搜索匹配点时，不必在整张图上盲目搜索，只需要沿这条线搜索，大幅降低匹配难度。
2）本质矩阵与相机标定
如果两台相机的内参 \((K, K’)\) 已知，则可以定义 本质矩阵（Essential Matrix） \(\displaystyle E\)：
\[ E = K’^{\top} F K \]
本质矩阵与相机间的相对旋转 \(\displaystyle R\) 与平移 \(\displaystyle t\) 直接相关。通过估计 \(\displaystyle F\) 再结合内参，就能恢复两台相机的相对位姿（最多有少数离散歧义）。(维基百科)
对于机器人而言，这意味着：
- 只要能在两帧图像中匹配到足够多的特征点，就能估计相机的运动（视觉里程计）；
- 再结合多帧图像的 3D 点重建，就能逐步构建环境地图——这就是视觉 SLAM 背后的核心几何。
3.3.1.3 三角测量
有了多视角、相机位姿和匹配到的特征点，对应的三维点怎样求出来？答案就是 三角测量（Triangulation）。(ics.uci.edu)
1）几何直觉
对同一个 3D 点 \(\displaystyle X\)，从相机中心 \(C_1\) 和 \(C_2\) 发出的两条光线分别“穿过”像平面上的点 \(\displaystyle x\) 和 \(x’\)。理论上，这两条光线在空间中会相交于 \(\displaystyle X\)。
【图 3-3 占位：两相机三角测量几何示意图】 图中画出两个相机中心、两条投影光线以及三维点交点，示意通过两条线求交得到 3D 位置。
在无噪声理想情况下，直接求两条直线的交点即可。但真实世界中，由于检测误差、标定误差，光线往往不会完全相交，而是“错开”一小段距离。
2）线性三角测量（概念层面）
在齐次形式下，两相机投影矩阵分别为 \((P_1, P_2)\)，图像点为 \((x, x’)\)。由于
\[ x \sim P_1 X,\quad x’ \sim P_2 X \]
可以写出一组关于 \(\displaystyle X\) 的线性方程组，通过最小二乘求解，使得重投影误差最小。这就是经典的线性三角测量方法。(ics.uci.edu)
对具身智能来说，你可以简单记住：
“三角测量”就是：给我两个视角下同一个像素点的位置 + 两个相机是如何摆放的，我就能算出这个点在三维世界大概在哪里。
后续在 3.3.2 的立体视觉深度中，我们会看到三角测量在规则双目系统中可以简化成非常漂亮的一个公式。
3.3.2 深度估计与点云表示
前面的多视角几何更多关注“稀疏的点”：少量特征点的 3D 恢复。对于机器人操作与导航，更常用的是：为每个像素估计一个深度（生成深度图），再把整个场景表示成点云 。
3.3.2.1 双目视觉深度
1）视差与深度公式
考虑一对平行摆放、经过极线校正的双目相机，左右相机的光轴大致平行，基线长度为 \(\displaystyle B\)，焦距为 \(\displaystyle f\)。同一 3D 点在左右图像上投影为 \(\displaystyle (u_L, v)\) 和 \(\displaystyle (u_R, v)\)。定义 视差（disparity）：
\[ d = u_L - u_R \]
经典双目几何推导给出深度公式：
\[ Z = \frac{fB}{d} \]
其中 \(\displaystyle Z\) 是点到相机平面的距离。可见视差越大（左右像素差越大），物体离相机越近；视差越小，物体越远。(teledynevisionsolutions.com)
【图 3-4 占位：立体双目几何与视差示意图】 画出左右相机、基线 \(\displaystyle B\)、焦距 \(\displaystyle f\)、空间点以及左右图像中的投影位置，标出视差 \(\displaystyle d\) 与深度关系。
2）视差计算与极线约束
要利用公式估深度，需要为每个像素找到左、右图之间的 对应点。在极线校正后，对应点应该在同一行上，因此只需沿水平方向搜寻视差，大大简化计算。(ni.com)
实际算法会：
- 使用块匹配或代价聚合（SGBM 等）在左右图上匹配；
- 对每个像素估计一个视差值 \(\displaystyle d\)，形成视差图；
- 再用 \(Z=\frac{fB}{d}\) 变换成深度图。
深度分辨率与 \((B, f)\) 和视差测量精度密切相关——基线越大、焦距越长，近距离深度精度越高，但视野和最大可测距离也会发生变化。(ni.com)
3）双目与其他深度传感器的比较
- 双目优点：只需两台普通 RGB 相机，成本较低，室外和远距离适用；
- 双目缺点：纹理少、强反光区域容易匹配失败；远距离时视差很小，深度噪声大；
- 其他选择：结构光 / ToF 深度相机、LiDAR 等将会在后续点云表示与机器人章节中频繁出现。
3.3.2.2 单目深度估计
1）纯几何角度：多帧单目结构恢复
如果只有单目相机，但有一段时间序列（视频），可以利用相机的运动产生视差：相机移动时，每一帧可以看作一个新的“视角”，配合估计的相机位姿，再通过三角测量得到稀疏或稠密深度。这是 单目视觉 SLAM / SfM 的传统思路。(IJCAI)
2）深度学习的单帧 / 视频深度估计
近年来更主流的路线是：直接训练一个网络，从单张图像或短视频片段预测每个像素的深度 。
- 监督式方法使用有真值深度的数据集（如 LiDAR 测量）进行回归；
- 自监督方法则通过 视图合成 作为监督信号：给定相邻帧图像、预测的深度与相机相对位姿，重投影前后帧，让网络最小化光度重建误差，而不需要真值深度标签。(CVF开放获取)
例如经典的自监督单目深度工作将深度估计网络和位姿估计网络一起训练，使得从视频中学习出几何一致的深度图和相机运动。
同时，基于 Vision Transformer 的单目深度网络（如 DPT，Dense Prediction Transformer）在大规模数据上预训练后，能够在不同场景中提供较为准确且平滑的深度估计。(arXiv)
3）单目深度的特点
- 优点：只需一台普通 RGB 相机；可以离线地从互联网视频或机器人日志中学习，适合大规模数据预训练；
- 局限：绝对尺度存在不确定性（除非有额外尺度信息），预测容易受训练数据分布影响；对纹理贫乏区域仍然困难。
在机器人系统中，单目深度估计常与 SLAM、结构化光或额外传感器结合使用，用于补充或增强深度感知。
3.3.2.3 点云
1）什么是点云？
点云（Point Cloud） 是一组三维点的集合，每个点通常包含：
\[ (x, y, z)\quad (+\text{颜色/强度/法向等属性}) \]
点云可以来自：
- 立体视觉/单目深度估计生成的深度图（每个像素反投影为一点）；
- LiDAR、闪光 LiDAR 等测距设备；
- 结构光或 ToF 深度相机。(SpringerLink)
【图 3-5 占位：室内场景点云可视化】 展示一个简单室内场景（桌子、椅子、墙壁）的彩色点云，点的稠密程度中等，便于读者直观感受点云是什么。
2）有序点云与无序点云
- 有序点云：由深度图反投影而来，点的排列与像素网格一致（可以看作带深度的图像）；
- 无序点云：由 LiDAR 或多视角融合得到，点通常是不规则散布的。
有序点云便于和图像特征对齐，无序点云则更适合全局建图、三维重建与高精度几何计算。
3）点云在机器人中的作用
- 建图与定位：对连续帧点云进行配准（如 ICP、NDT 等算法），构建三维地图并估计机器人位姿，是 3D SLAM 的核心。(维基百科)
- 障碍物检测与避障：从点云中分割出地面、障碍物，给路径规划模块使用。(sensors.myu-group.co.jp)
- 物体识别与抓取：在点云中拟合几何形状（平面、圆柱、盒子），或基于点云特征进行 3D 检测与 6D 位姿估计。(arXiv)
在后面的 3.3.3 和 3.3.4 节中，我们会看到点云如何与物体检测、抓取和避障紧密结合。
3.3.3 物体检测与 6D 位姿估计
机器人若要“抓住某个物体”，光知道“画面里有个杯子”是不够的，还必须知道：杯子在三维空间中的位置和朝向 。这由 6D 位姿估计 提供。
3.3.3.1 6D 位姿
1）6 自由度位姿的定义
6D 位姿（6-DoF pose）通常指刚体在三维空间中的：
- 3 维平移：\(\displaystyle (x, y, z)\)，位置；
- 3 维旋转：可以用欧拉角（roll/pitch/yaw）、旋转矩阵 \(\displaystyle R\)、四元数 \(\displaystyle q\) 等表示。
我们经常直接写成一个齐次变换矩阵：
\[ T = \begin{bmatrix} R & t\ 0 & 1 \end{bmatrix} \]
在 6D 物体姿态估计问题中，我们关心的是物体相对于 某个坐标系 的位姿，常见为相机坐标系或者机器人基座坐标系。(arXiv)
2）为什么 6D 位姿对机器人如此关键？
- 抓取：爪子从哪个方向接近物体、如何旋转才能避免碰撞，完全取决于物体的 6D 姿态；
- 精密操作：如插入插座、对接零件，要求物体与工具之间的姿态误差在毫米/角度量级；
- 多物体操作：在堆叠、整理、装箱任务中，需要准确知道每个物体的空间布局。
从具身智能宏观角度看，6D 位姿估计可以视为连接“视觉理解”和“工程控制”的关键桥梁，是许多机器人抓取系统的前置步骤。(arXiv)
3.3.3.2 物体检测 + 姿态估计流水线
现实系统中，6D 位姿估计通常建立在 2D 物体检测的基础上形成一个 流水线。可以概括为以下几个步骤：
1）2D 物体检测或分割
使用目标检测或实例分割网络（如 YOLO、Faster/Mask R-CNN 等），识别出图像中的物体类别与 2D 边界框 / 像素掩膜。此时我们知道 哪里 有一个“杯子”或“方块”，但还不知道它的空间姿态。
2）建立 2D–3D 对应关系
根据物体类型，通常有以下几种方式建立 2D–3D 对应：
- 已知 CAD 模型或模板：在模型坐标系中选取一组 3D 特征点（如角点），网络预测这些点在图像中的 2D 位置。
- 类别级 3D 盒子：网络预测物体的 3D 包围盒形状和方向，从而隐含给出一些“虚拟”3D 点与 2D 点的对应关系。(MDPI)
3）求解 PnP 或直接回归位姿
有了若干个对应的 3D 点 \(({X_i})\) 和像素点 \(({x_i})\)，再加上相机内参 \(\displaystyle K\)，就可以通过 PnP（Perspective-n-Point） 问题求解物体相对于相机的旋转 \(\displaystyle R\) 和平移 \(\displaystyle t\)。(r-5.org)
另一类方法则直接用神经网络回归 6D 位姿参数，然后再结合渲染或 ICP 进行几何优化。
4）利用深度 / 点云进行精细对齐
如果有深度图或点云，可以将 CAD 模型渲染到深度图中，与观测到的点云对齐，利用 ICP 或概率配准（如 NDT）进行 pose refinement，以提升位姿精度，尤其是对遮挡部分的补偿。(维基百科)
【图 3-6 占位：典型“检测 + 6D 位姿估计”流水线示意图】 用流程图展示：输入 RGB/深度 → 2D 检测 → 关键点/分割 → PnP 解位姿 → 利用点云 refine → 输出 6D pose。
整体来看，这条流水线帮助机器人从“这张图里有一个杯子”上升到“杯子在相机前方 0.6m、右侧 0.2m、高度 0.1m，绕 z 轴转了 30°”。
3.3.3.3 应用
1）抓取与放置
在抓取任务中，6D 位姿估计最直接的用途是为运动规划提供目标约束。例如：
- 已知某个物体相对于相机的姿态 \(T_{\text{obj}}^{\text{cam}}\)，再结合相机-机械臂的手眼标定 \(T_{\text{cam}}^{\text{base}}\)，可求出物体在机器人基座坐标系下的位姿；
- 运动规划器据此生成一条无碰路径，让末端执行器在期望的姿态下接近并抓住物体。(arXiv)
2）装配、对接与精确操作
对于插拔连接器、插销入孔等操作，稍有角度偏差就会导致卡顿甚至损坏。6D 位姿估计结合高分辨率点云，可以提供毫米级的位姿测量，为高精度装配与工业机器人应用提供基础。(arXiv)
3）多物体场景理解与 VLA
在更复杂的场景中，6D 位姿与 3D 检测结果可以构成一个“物体级 3D 场景图”：每个物体都有类别标签和空间位姿。这种表示与语言指令（“把右边那个蓝色杯子放到桌子上”）天然契合，为后续第 8 章中的 VLA 模型提供结构化的感知输入。(arXiv)
3.3.4 对机器人特别重要的视觉能力（可抓取检测、障碍物检测等）
前面介绍的多视几何、深度估计和 6D 位姿，为机器人提供了“几何级”的理解。本节聚焦几个在具身智能中极为关键、又高度依赖前面几何基础的 专项视觉能力：可抓取检测、障碍物检测和场景语义理解。
3.3.4.1 可抓取检测
1）从“物体姿态”到“抓取姿态”
6D 位姿估计告诉我们“物体在哪里、怎么放”，但机器人真正执行的是“手爪在哪里、怎么放”。可抓取检测（grasp detection） 更关心的是：
在当前场景几何和任务需求下，哪些手爪位姿是“可行且安全”的抓取姿态？
早期方法常用二维抓取矩形来表示抓取，即在图像平面上预测一个长方形 \(x, y, w, h, \theta\)，矩形中心是抓取中心，角度 \(\theta\) 决定手爪闭合方向。(arXiv)
在 3D 中，抓取姿态则通常包括：抓取接触点的 3D 坐标、抓取方向（法向）、手爪张开宽度以及手爪自身的姿态——本质上就是一个针对末端执行器的 6D pose。(ScienceDirect)
2）基于深度学习的抓取检测
现代抓取检测普遍采用深度学习，从 RGB-D 图像或点云输入直接预测一组抓取候选及其“成功概率”：
- 将场景映射到一个 graspability map：每个像素或点都有“可抓概率”，再从中选取评分高的局部作为抓取目标；(arXiv)
- 使用 CNN/GCN 等对点云或图像进行编码，预测一批 6-DoF 抓取姿态和对应的评分。(ScienceDirect)
【图 3-7 占位：可抓取检测示意图】 上图：叠放杂物的桌面；中图：可抓取热力图（颜色越红越容易抓）；下图：选出的若干 3D 抓取姿态用绿色手爪示意。
3）可供性（Affordance）视角
与其先识别物体类别再决定如何抓取，另一种更“具身”的视角是：直接学习场景中哪些区域 具有某种操作可供性（affordance），比如“可抓”、“可按”、“可拉”。
- 这类方法预测每个像素 / 点的任务相关可供性标签，再生成相应的抓取或操作动作；(cs.columbia.edu)
- 对 VLA 模型而言，可供性图可以看作把视觉输入转化为“行动机会”的中间表征，有利于在复杂任务中做高层决策。
3.3.4.2 障碍物检测
机器人不仅要“抓到东西”，还必须“别撞到东西”。障碍物检测 是所有具身系统最基本的安全能力之一。
1）移动机器人：从深度 / 点云到占据图
对于轮式移动机器人或无人车，常见流程是：
- 使用双目、LiDAR 或其他深度传感器获取周围环境的点云或深度图；
- 将这些点投影到地平面坐标系下，构建 占据栅格（occupancy grid） 或 voxel map，标记哪些单元格为“被障碍物占据”；
- 在此基础上，局部规划算法（如 VFH、DWA）生成避免障碍的安全路径。(维基百科)
有些工作会用更加紧凑的场景表示，如 stixels：将深度信息压缩为一系列垂直“柱子”，每根柱子表示在该方向上的最近障碍物，大大提升了处理效率。(维基百科)
2）机械臂工作空间内的障碍物
对固定底座的机械臂来说，障碍物通常分为：
- 静态环境，例如桌边、墙面、固定装置；
- 动态物体，例如其他机器人、人或者当前操作的物品。
利用机载 RGB-D 相机或外部 3D 传感器，可将障碍物点云融合到机器人坐标系下，交给运动规划器（如基于 RRT* 或轨迹优化的规划器），在规划路径时用碰撞检测排除会穿过障碍物的轨迹。(ScienceDirect)
3）透明 / 复杂障碍物
现实环境中，玻璃、亚克力板等透明障碍物对 RGB 和深度相机都是难点。最近有工作利用 LiDAR 点云 强度 信息和多层网格表示，专门针对透明障碍物进行检测，以提升室内机器人导航的安全性。(arXiv)
3.3.4.3 场景语义理解
仅仅知道“哪儿有障碍物”还不够。具身智能系统还需要理解：哪儿是桌面、哪儿是抽屉、哪儿是垃圾桶、哪儿是不该进入的区域 。这就是 场景语义理解 的任务。
1）从像素到语义：2D/3D 语义分割
在 2D 视角上，语义分割模型为每个像素分配类别标签（桌子、地面、墙、物体种类等）；结合深度信息或点云，可将这些语义标记提升到 3D，构建 语义点云 或 语义体素地图。(CVF开放获取)
2）多层次场景表示
一个对机器人有用的场景表征往往包含多层信息：
- 几何层：深度图、点云，提供可通行空间和障碍物；
- 物体层：各个物体的 6D 位姿和形状；
- 语义层：物体和区域的类别及功能（桌子、垃圾桶、储物箱、禁区等）。
例如，多视图融合系统可以从一系列“相机在手”的 RGB 图像中构建场景点云、估算未知物体的大致形状，并对已知物体估计精确 6D 姿态，为机器人操作提供全方位信息。(arXiv)
3）与语言和任务的结合
语义理解是后续“语言——视觉——行动”闭环的关键支撑：
- 当用户说“把桌子上的红杯子放进垃圾桶”，语义分割与检测为 VLA 模型提供“桌子”“杯子”“垃圾桶”等概念在场景中的位置；
- 高层规划可以基于“区域”而不是“裸坐标”思考：先找到桌面区域，再在桌面上搜索目标，再规划路径到垃圾桶区域。
这部分将在第 4 章语言与多模态表示、以及第 8 章 VLA 架构中被系统化地使用。本节的角色是：为具身智能提供一套能够支撑复杂任务的几何与语义感知基础 。
到这里，你可以把 3.3 这一大块理解为：
我们已经从“相机如何把 3D 看成 2D”一路走到“机器人如何从多视角、多传感器中重建 3D 世界，并识别其中可以抓取的区域、需要避开的障碍物和具有特定语义的区域”。
接下来 3.4 节会进一步讨论这些视觉能力在真实机器人系统中的工程落地：相机标定、延迟、噪声、帧率等实际问题。