2. 三维几何基础
在机器人3D场景图建模中，所有感知、定位、建图与规划模块最终都要落到统一的几何表达上。点云、网格、体素、深度图是四种最常见的三维数据形态，相机模型负责把三维世界投影到二维图像，而坐标系变换与刚体运动表示则把不同传感器、不同时刻的观测统一到同一参考系。理解这些基础对象之间的转换关系，是后续场景图节点、边和属性设计的根基。
一、四种三维几何表达及其适用场景
点云是三维空间中的离散点集合，每个点通常包含 x、y、z 坐标，并可附带强度、颜色、法向量等属性。它最接近激光雷达、深度相机的原始输出，适合表达不规则表面和远距离稀疏观测，但缺少显式拓扑连接，直接用于碰撞检测或渲染时效率较低。
网格由顶点、边和三角面片组成，显式表达物体表面连续性和封闭性。网格适合可视化、物理仿真和表面法向计算，但重建成本高，对噪声和缺失区域敏感，动态场景中维护代价较大。
体素把空间划分为规则的小立方体，用占据、自由或未知状态描述环境。体素天然支持空间索引、碰撞查询和概率更新，是导航地图与OctoMap类建图的常用形式，但分辨率提高时内存增长呈立方级。
深度图是相机视角下的二维图像，每个像素记录到场景表面的距离。它保留了图像的行列结构，便于卷积网络处理，但受视角限制，单帧深度图无法直接获得物体完整几何。
二、相机模型：从三维点到像素
针孔相机模型是最常用的投影模型。设相机坐标系下一点为 P_c = (X, Y, Z)，像素坐标为 (u, v)，则投影关系为：
u = fx * X / Z + cx
v = fy * Y / Z + cy
其中：
fx, fy 为焦距（像素单位）
cx, cy 为主点坐标
Z 为深度值实际镜头存在径向畸变和切向畸变，通常用 k1, k2, p1, p2, k3 等系数描述。去畸变后再进行投影，才能保证点云与图像像素的准确对应。相机内参矩阵 K 把归一化平面映射到像素平面：
K = [ fx 0 cx
0 fy cy
0 0 1 ]外参则描述相机相对世界坐标系的位姿，由旋转矩阵 R 和平移向量 t 组成。内参决定“怎么投影”，外参决定“从哪里看”。
三、坐标系变换与刚体运动表示
机器人系统中常见坐标系包括世界系、机器人基座系、相机系、激光雷达系、末端执行器系等。任意两个坐标系之间的变换都是刚体变换，即只包含旋转和平移，不改变距离和角度。
齐次变换矩阵 T 是最通用的表示：
T = [ R t
0 1 ]
R 为 3x3 旋转矩阵，t 为 3x1 平移向量
点变换：P_A = T_AB * P_B旋转的表示方法各有取舍：旋转矩阵直观但冗余且需正交约束；欧拉角紧凑但存在万向节死锁；轴角表示适合插值；四元数 (w, x, y, z) 无奇异、插值平滑，是机器人中最常用的旋转表示。四元数需保持单位长度，否则会引入缩放误差。
| 表示方法 | 自由度 | 优点 | 缺点 | 典型用途 |
|---|---|---|---|---|
| 旋转矩阵 | 9参数/3自由度 | 线性变换直观 | 冗余、需正交化 | 变换矩阵组合 |
| 欧拉角 | 3 | 人类易读 | 万向节死锁 | 界面显示、简单姿态 |
| 轴角 | 3+1 | 几何意义明确 | 插值不自然 | 旋转轴分析 |
| 四元数 | 4 | 无奇异、插值平滑 | 需归一化、不直观 | SLAM、姿态估计 |
四、从深度图到点云：一次完整的几何转换
以RGB-D相机为例，给定像素 (u, v) 和深度 Z，反投影得到相机坐标系下的三维点：
X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
Z = Z
再通过外参 T_wc 变换到世界系：
P_w = T_wc * P_c这一过程是场景图建模中“像素—点—体素—节点”链路的起点。若深度图存在空洞或噪声，反投影后的点云会出现离群点和缺失区域，需要滤波、补全或体素下采样后再进入场景图构建。
五、典型误区与复盘
- 混淆坐标系方向：相机系通常 z 向前、x 向右、y 向下，而机器人基座系常为 x 向前、y 向左、z 向上。未统一轴向会导致点云“躺倒”或镜像。
- 忽略畸变：直接用理想针孔模型处理广角镜头图像，边缘像素反投影误差可达数厘米，直接影响场景图节点位置精度。
- 四元数未归一化：多次乘法后四元数模长漂移，旋转矩阵出现缩放，导致点云逐渐变形。
- 体素分辨率选择不当：分辨率过粗会丢失小物体，过细则内存和计算量急剧上升，需结合场景图节点粒度反推。
- 深度图与彩色图未对齐：RGB 与深度来自不同镜头时，必须使用对齐后的深度图，否则颜色与几何错位。