点云拼接 (RGB-D 点云建图)

把一组 RGB-D 图像（彩色图 + 深度图）和对应的相机位姿，拼接成一个彩色 3D 点云地图。对应《视觉 SLAM 十四讲》第 5 讲 "实践：拼接点云" 的 Python 实现。

「2026 SLAM 点云拼接 14讲作业」

/~1a133ZPR8a~:/

链接：https://pan.quark.cn/s/31231b18c9eb

原书用 C++ + PCL/OpenCV/Eigen，本仓库改为纯 Python（NumPy + OpenCV + Matplotlib），并额外提供 open3d 交互式查看器。

一、目录结构

复制代码

点云拼接/
├── frame/                # 彩色图 (frame001970.jpg ~ frame001999.jpg，30 帧，1200×680)
├── depth/                # 深度图 (depth001970.png ~ depth001999.png，uint16，单位 mm)
├── traj.txt              # 相机位姿真值（2000 行 4×4 齐次矩阵，帧 N 对应第 N 行）
├── join_map.py           # 主程序：拼接点云，输出 map.pcd / map.ply / map_view.png / poses.txt
├── viewer.py             # open3d 交互式点云查看器（鼠标旋转/缩放查看细节）
├── map.pcd               # 输出：拼接结果（PCL 兼容，XYZRGB binary）
├── map.ply               # 输出：拼接结果（open3d 格式）
├── map_view.png          # 输出：3 视角投影图（透视/俯视/侧视）+ 相机轨迹
├── poses.txt             # 输出：本数据集 30 帧对应的位姿（tx ty tz qx qy qz qw）
├── 教学文本.txt          # 原书该节内容（参考说明）
└── README.md             # 本文件

说明：map.pcd / map.ply / map_view.png / poses.txt 是程序运行后自动生成的产物，首次使用可先删除它们，再按下面步骤重新生成。

二、环境准备

需要 Python 3.8+，安装依赖：

复制代码

pip install numpy opencv-python matplotlib scipy

交互式查看器额外需要 open3d：

复制代码

pip install open3d

已验证环境：Windows 10 + Python 3.9.5 + numpy 1.26 + opencv 4.11 + matplotlib 3.7 + open3d 0.19。 Windows 下若 open3d 因 dash/comm 报 NotImplementedError，本仓库的 join_map.import_open3d() 已做兼容处理（自动 stub 冲突模块），无需手动干预。

三、快速开始

1. 拼接点云

在项目根目录执行：

复制代码

python join_map.py

输出示例：

复制代码

找到 30 对 RGB-D 图像（帧号 1970~1999）
已从 traj.txt 读取 30 个真实相机位姿

拼接点云...
  帧 5/30 (idx=1974): 累计 +50998 点
  ...
合并后点云共 1529886 个点
降采样后点云共 392959 个点

保存 map.pcd / map_view.png / poses.txt ...
  可视化已保存到 map_view.png
  额外保存 map.ply（open3d 格式）
  提示：运行 python viewer.py 打开交互式 3D 窗口查看细节

2. 查看点云（三种方式，任选其一）

方式 A：open3d 交互窗口（最推荐，能看清细节）

复制代码

python viewer.py            # 默认读 map.ply
python viewer.py map.pcd    # 指定文件
python viewer.py --traj     # 叠加红色相机轨迹

窗口内操作：

操作	功能
鼠标左键拖动	旋转视角
鼠标右键拖动	平移
滚轮	缩放
`R`	自动重置到整体视角
`+` / `-`	放大 / 缩小点的大小
`W`	切换坐标轴显示
`H`	打印完整快捷键帮助
`Q` / `Esc`	退出

方式 B：直接看投影图

复制代码

打开生成的 map_view.png（透视 + 俯视 X-Y + 侧视 Y-Z 三视角，含相机轨迹）。

方式 C：第三方工具

map.pcd 是标准 PCL 格式，可用：

CloudCompare（免费、跨平台、推荐）
MeshLab
pcl_viewer map.pcd（PCL 自带命令行工具，Linux）

四、原理

对每帧 RGB-D 图像的每个像素 (u, v)：

反投影到相机坐标系 （利用内参 K）：

复制代码

z = depth(u,v) / depth_scale
x = (u - cx) * z / fx
y = (v - cy) * z / fy

变换到世界坐标系 （利用外参 T_wc，即相机在世界系下的位姿）：
复制代码
```
p_world = T_wc · [x, y, z, 1]ᵀ
```
把所有帧的全部像素合并、体素降采样，得到拼接后的彩色地图。

本数据集的参数

参数	值	说明
图像尺寸	1200 × 680
`fx`, `fy`	600.0, 600.0	估算值（无 EXIF/标定文件；若有真实内参请改 `join_map.py` 顶部）
`cx`, `cy`	600.5, 340.5	主点，约图像中心
`depth_scale`	5000	`depth_value / 5000 = 米`（TUM 标度）
帧范围	1970 ~ 1999	共 30 帧
位姿来源	`traj.txt` 第 1970~1999 行	真值位姿（4×4 齐次矩阵）

五、常见问题

Q1: cv2.imread 读不到带中文路径的图？ join_map.py 已用 cv2.imdecode(np.fromfile(...)) 替代，原生支持中文路径。

Q2: open3d 报 NotImplementedError: Cannot ...？ 是 dash/comm 在非 notebook 环境的已知冲突。viewer.py 通过 join_map.import_open3d() 自动绕过，直接运行 python viewer.py 即可。

Q3: 点云看起来有重影 / 错位？ 通常是内参或深度尺度不准。可调整 join_map.py 顶部的 FX/FY/CX/CY 与 DEPTH_SCALE。

Q4: 想自己估计位姿（没有 traj.txt）？ 本仓库用真值位姿以保证拼接质量。若需要练习位姿估计，可参考 RGB-D ICP / ORB 里程计的实现思路自行扩展。

这是一个基于 RGB-D 相机的三维点云拼接与建图示例工程，参考《视觉 SLAM 十四讲》第 5 讲实现。项目利用 30 组彩色图与深度图，结合相机轨迹真值，将多帧图像反投影为空间点并转换至世界坐标系，最终生成包含颜色信息的稠密 3D 点云地图。支持输出 PCD、PLY 等标准格式，并提供多视角投影图与交互式查看器，便于观察场景结构与相机运动。代码包含完整的工程目录、依赖说明与可视化操作指引，适合学习 RGB-D SLAM 中点云生成、坐标变换及多帧拼接的核心流程，也可作为视觉建图与三维重建的教学与实践基础。