传感器基础：深度相机与里程计
做SLAM这么多年，我越来越觉得传感器选型是个「一着不慎，满盘皆输」的活。你算法再牛，传感器数据拉胯，后面全是白搭。今天咱们就聊聊室内场景里最常用的几种传感器——深度相机、IMU、轮式里程计，以及它们背后的标定门道。
一、深度相机：三种主流方案
深度相机说白了就是能告诉你「像素离我有多远」的相机。室内SLAM里，它几乎是标配。目前主流方案有三种：结构光、ToF、双目。我挨个说说。
1. 结构光（Structured Light）
原理很简单：投影仪打出已知图案（比如红外散斑），相机拍下图案的变形，通过三角法算出深度。Kinect v1就是典型代表。
核心公式（三角测距）：
Z = (f * b) / (x_left - x_right)其中 Z 是深度，f 是焦距，b 是基线，x_left - x_right 是视差。
我个人习惯在室内小场景（3-5米内）用结构光。精度确实高，能达到毫米级。但有个致命伤——怕强光。我在一个阳光直射的落地窗旁测过，散斑直接被环境光淹没了，深度图一片漆黑。
避坑指南：我曾经在户外半开放阳台做实验，结构光相机直接罢工。后来才意识到，它的红外投影在阳光下就是个弟弟。室内用没问题，室外请绕道。
2. ToF（Time of Flight）
ToF的原理更直接——发射光脉冲，测量反射回来的时间。光速已知，时间一测，距离就有了。你想想看，这不就是激光雷达的平民版吗？
ToF的优势是速度快，能到30fps甚至更高。而且对光照没那么敏感。我试过在暗光环境下用ToF，效果比结构光好太多。但它的分辨率普遍偏低，VGA（640×480）就算不错了。另外，多台ToF设备同时工作会互相干扰——这个坑我踩过。
小技巧：如果你用ToF做动态物体检测，记得开「多路径干扰抑制」。很多SDK默认没开，开了之后边缘拖影会好很多。
3. 双目立体视觉（Stereo Vision）
双目就是模仿人眼——两个相机拍下左右两张图，通过特征匹配找到对应点，再用三角法算深度。纯被动式，不发射任何光。
双目最大的好处是：室内室外通吃。我在一个半室外走廊测试过，结构光和ToF都翻车了，双目反而稳得很。但它的计算量是真的大——每帧都要做立体匹配。而且对纹理敏感，白墙这种无纹理区域，匹配会失败。
| 方案 | 精度 | 范围 | 抗光性 | 计算量 | 典型代表 |
|---|---|---|---|---|---|
| 结构光 | 高（mm级） | 0.1-5m | 差 | 低 | Kinect v1, Intel RealSense D415 |
| ToF | 中（cm级） | 0.1-10m | 中 | 低 | Kinect v2, PMD, 索尼DepthSense |
| 双目 | 中高（依赖基线） | 0.3-20m+ | 好 | 高 | ZED, MYNT EYE, 大疆双目 |
二、IMU与轮式里程计
深度相机再牛，也有短板——快速运动时容易丢帧。这时候就需要里程计来「救场」了。
1. IMU（惯性测量单元）
IMU包含加速度计和陀螺仪。加速度计测线加速度，陀螺仪测角速度。通过对加速度积分得速度，再积分得位移——听起来很美好，对吧？
但现实很骨感。IMU有零偏、有噪声，积分一次漂移一点，两次漂移一大截。我做过一个实验：把IMU放在桌上静止10分钟，积分出来的位置已经跑到隔壁房间了。所以IMU不能单独用，必须和视觉/激光做融合。
IMU预积分（Preintegration）：这是VINS-Mono等系统的核心技巧。把两帧图像之间的IMU数据预先积分成一个约束，避免重复积分。公式长这样：
ΔR_ij = ∏ exp((ω_k - b_g) * Δt)
Δv_ij = Σ R_k * (a_k - b_a) * Δt
Δp_ij = Σ [Δv_k * Δt + 0.5 * R_k * (a_k - b_a) * Δt²]2. 轮式里程计
轮式里程计更接地气——通过轮子转的圈数推算位移。编码器每转一圈发N个脉冲，数脉冲就知道走了多远。
公式很简单：
位移 = 脉冲数 / N * 轮子周长但这里有个坑：轮子打滑。我在瓷砖地面上测试过，急加速时轮子空转，里程计显示走了1米，实际只走了0.8米。所以轮式里程计适合室内平整地面，地毯或瓷砖都行，但别指望它多精准。
避坑指南：我曾经在AGV上只用轮式里程计做定位，结果过了一个门槛就偏了30度。后来加了IMU做航向修正，才稳住。记住：轮式里程计+IMU是低成本方案的最佳拍档。
三、传感器标定基础
传感器买回来不能直接用——你得告诉它「你是谁，你在哪」。这就是标定。
1. 内参标定
相机内参包括焦距(fx, fy)、光心(cx, cy)、畸变系数(k1, k2, p1, p2)。标定方法就是拍棋盘格，用张正友标定法。
我个人习惯用Kalibr或ROS的camera_calibration包。操作很简单：打印一张棋盘格，拿着在相机前晃一晃，采集20-30张不同角度的图像，跑一下脚本就出结果。
# ROS标定命令示例
rosrun camera_calibration cameracalibrator.py \
--size 8x6 --square 0.024 \
image:=/camera/image_raw2. 外参标定
外参就是传感器之间的相对位姿。比如深度相机和IMU之间，到底谁在谁的什么方向？
常用的工具有：
- Kalibr：支持多相机+IMU联合标定，输出旋转矩阵和平移向量
- lidar_camera_calibration：激光和相机的标定
- 手眼标定：机器人臂上传感器的标定，AX=XB问题
小技巧：标定IMU-camera时，记得让设备做「充分激励」——六个自由度都动一动，别只在一个平面晃。我见过有人只转不平移，结果标出来的外参偏了5度。
3. 联合标定
多传感器融合时，联合标定是绕不开的。比如深度相机+IMU+轮式里程计，三个传感器的时间戳要对齐，空间位姿要统一。
时间同步是个大问题。不同传感器的时钟源不同，ROS里用time_synchronizer做近似对齐。但如果你要亚毫秒级精度，得用硬件触发——比如把相机的帧同步信号接到IMU的中断引脚上。
联合标定流程：
- 分别标定每个传感器的内参
- 两两标定外参（相机-IMU、相机-轮式）
- 构建因子图，优化所有外参
- 验证：投影误差<1像素，IMU零偏稳定
嗯，传感器这块内容确实不少。但记住一句话：标定做不好，SLAM全白搞。我见过太多项目，算法调得飞起，结果发现是标定参数错了——那种感觉，就像你跑完马拉松发现跑错了赛道。
好了，这一章就到这里。下一章咱们聊聊数据预处理和特征提取，那是SLAM管线的第一道关卡。