📷 一、系统架构概述
👁️👁️
双目单色相机
用于立体匹配
获取深度信息
🎨
RGB彩色相机
用于纹理采集
提供颜色信息
💡 为什么这样设计？
- 双目mono：单色传感器对比度高、噪声低，立体匹配更准确
- RGB单独：彩色相机分辨率高、色彩准确，纹理质量好
- 分工合作：各司其职，既有精确深度又有真实色彩
完整工作流程
双目采图
→
立体匹配
→
3D重建
↓
坐标转换
→
畸变投影
→
RGB贴图
🔄 二、坐标系转换详解
2.1 坐标系关系链
世界坐标系 (标定板)
↓
原始左相机坐标系 ←─[R_rgb, T_rgb]─ RGB相机坐标系
↓ ↑
[R_rect 极线矫正] |
↓ |
矫正后左相机坐标系 |
↓ |
【点云在这里】────────需要转换───────────┘
🎯 核心问题
点云在矫正后左相机坐标系，但RGB相机的标定外参是相对原始左相机的。
我们需要建立从矫正后坐标系到RGB坐标系的桥梁。
2.2 什么是极线矫正？
物理含义：把两个相机的图像平面"对齐"到同一平面，使对应点在同一行
数学表达：对图像进行重映射，等效于相机旋转了角度 R_rect
结果：立体匹配只需沿水平方向搜索，速度快10倍以上
⚠️ 关键理解：
极线矫正是虚拟旋转，相机物理位置没变，但数学上等效于坐标系旋转了 R_rect。
2.3 转换推导过程
第一步：理解矫正变换
stereoRectify() 返回 R1 (左相机矫正旋转矩阵)
含义：P_rect = R1 × P_orig
其中：
P_orig: 点在原始左相机坐标系
P_rect: 点在矫正后左相机坐标系
第二步：逆矫正（关键）
我们的点云在矫正后，要转回原始坐标系：
P_orig = R1^T × P_rect
为什么是转置？
→ R1 是旋转矩阵（正交矩阵）
→ 正交矩阵的逆 = 转置
→ 物理意义：相机"反转"回去
第三步：应用RGB外参
标定得到的外参(R_rgb, T_rgb)描述：
RGB相机相对原始左相机的位姿
转换公式：
P_rgb = R_rgb × P_orig + T_rgb
✅ 第四步：合并变换（最终公式）
代入消去中间变量 P_orig：
P_rgb = R_rgb × (R1^T × P_rect) + T_rgb
= (R_rgb × R1^T) × P_rect + T_rgb
最终变换矩阵：
R_final = R_rgb @ R1.T
T_final = T_rgb
🔢 矩阵维度验证
| 变量 | 维度 | 说明 |
|---|---|---|
| P_rect | (N, 3) | N个3D点 |
| R1 | (3, 3) | 矫正旋转 |
| R_rgb | (3, 3) | RGB外参旋转 |
| T_rgb | (3, 1) | RGB外参平移 |
| R_final | (3, 3) | R_rgb @ R1.T ✓ |
| P_rgb | (N, 3) | 转换后点云 ✓ |
🎯 三、考虑畸变的重投影
3.1 完整投影流程
3D点
(RGB坐标系)
→
(RGB坐标系)
归一化坐标
(x, y)
→
(x, y)
应用畸变
(x', y')
→
(x', y')
像素坐标
(u, v)
(u, v)
3.2 为什么要考虑畸变？
理想情况（无畸变）
- 直线投影仍是直线
- 网格保持规整
- 只在理论中存在
实际情况（有畸变）
- 广角镜头：桶形畸变
- 长焦镜头：枕形畸变
- 制造误差：切向畸变
⚠️ 不考虑畸变的后果：
- 图像边缘：投影误差可达 10-50像素
- 贴图错位：纹理严重扭曲变形
- 3D重建：几何精度大幅下降
3.3 详细投影步骤
步骤1️⃣：3D到归一化平面
给定点 P_rgb = [X, Y, Z] 在RGB相机坐标系
归一化（透视除法）：
x_norm = X / Z
y_norm = Y / Z
→ 这是理想针孔模型的坐标（无畸变）
步骤2️⃣：应用畸变模型
Brown-Conrady畸变模型（OpenCV标准）：
径向畸变（桶形/枕形）：
r² = x_norm² + y_norm²
radial = 1 + k1·r² + k2·r⁴ + k3·r⁶
切向畸变（透镜制造误差）：
dx = 2·p1·x_norm·y_norm + p2·(r² + 2·x_norm²)
dy = p1·(r² + 2·y_norm²) + 2·p2·x_norm·y_norm
合并畸变：
x_dist = x_norm · radial + dx
y_dist = y_norm · radial + dy
| 畸变系数 | 物理含义 | 典型值范围 |
|---|---|---|
| k1 | 2阶径向畸变 | -0.5 ~ 0.5 |
| k2 | 4阶径向畸变 | -0.1 ~ 0.1 |
| k3 | 6阶径向畸变 | -0.05 ~ 0.05 |
| p1, p2 | 切向畸变 | -0.01 ~ 0.01 |
步骤3️⃣：归一化到像素
使用RGB相机内参矩阵：
K_rgb = [fx 0 cx]
[ 0 fy cy]
[ 0 0 1]
像素坐标计算：
u = fx · x_dist + cx
v = fy · y_dist + cy
其中：
fx, fy: 焦距（像素单位）
cx, cy: 主点坐标（光轴与图像平面交点）
✅ 完整重投影公式总结
输入：点云 P_rect (N×3) 在矫正后左相机坐标系
输出：像素坐标 (u, v)
完整流程：
1️⃣ 坐标转换：
P_rgb = (R_rgb @ R1.T) @ P_rect.T + T_rgb
→ [X, Y, Z] 在RGB相机坐标系
2️⃣ 归一化：
x = X / Z
y = Y / Z
3️⃣ 畸变：
r² = x² + y²
radial = 1 + k1·r² + k2·r⁴ + k3·r⁶
x' = x·radial + 2p1·xy + p2(r²+2x²)
y' = y·radial + p1(r²+2y²) + 2p2·xy
4️⃣ 像素化：
u = fx·x' + cx
v = fy·y' + cy
⚙️ 四、完整工作流程
4.1 离线准备阶段（一次性）
步骤1：标定双目系统
- 拍摄20-30对棋盘格图像
- 获得：内参 K_left, K_right, 畸变 dist_left, dist_right
- 获得：外参 R_stereo, T_stereo（右相机相对左相机）
步骤2：标定RGB相机
- 单独拍摄20-30张棋盘格
- 获得：内参 K_rgb, 畸变 dist_rgb
步骤3：联合标定（关键！）
- 三个相机同时拍摄同一棋盘格
- 拍摄15-25组不同姿态
- 获得：R_rgb, T_rgb（RGB相对原始左相机）
步骤4：计算矫正参数
- 运行 stereoRectify 函数
- 获得：R1, R2（矫正旋转）, P1, P2（新投影矩阵）, Q（重投影矩阵）
- ⚠️ 一定要保存 R1！后续转换必需
步骤5：预计算变换矩阵（优化性能）
R_rect2rgb = R_rgb @ R1.T
T_rect2rgb = T_rgb
→ 保存这两个矩阵，运行时直接使用，避免重复计算
4.2 在线运行阶段（实时）
实时处理流程
输入：双目mono图像对 + RGB彩色图像（同步采集）
步骤1：双目矫正
使用预计算的映射表快速重映射
步骤2：立体匹配
计算视差图（SGBM/BM算法）
步骤3：视差转点云
使用Q矩阵重投影到3D（矫正后左相机坐标系）
步骤4：坐标转换
应用 R_rect2rgb 和 T_rect2rgb
步骤5：带畸变的投影
归一化 → 畸变 → 像素化
步骤6：提取颜色
有效性检查 + 双线性插值
输出：带RGB颜色的高质量点云
🔧 五、关键技术细节
5.1 有效性检查
投影点必须同时满足以下条件：
1. 深度检查：Z_rgb > 0
→ 点在RGB相机前方
2. 边界检查：
0 ≤ u < image_width
0 ≤ v < image_height
→ 投影在图像内
3. 深度合理性（可选）：
Z_min < Z_rgb < Z_max
→ 过滤异常噪声点
4. 视差有效性：
disparity > min_disparity
→ 立体匹配成功的点
5.2 时间同步
⚠️ 关键问题：
三个相机采集时刻不一致会导致动态场景中点云和纹理错位