RGB-D点云融合实战：三帧图像拼接完整点云的原理与实现

简介：一份面向三维重建与计算机视觉学习者的点云融合实践项目，对应作业13「多帧点云融合」实验。项目基于RGB-D相机拍摄的不连续三帧图像，借助4×4齐次变换矩阵，逐步完成点云生成、坐标变换、ICP配准与融合，直观展示多帧点云对齐与拼接的完整流程，适合入门三维重建技术的学习者动手演练。

压缩包共14个文件、约3.06MB，核心代码由2个C++源文件、1个hpp头文件和1个so动态库构成，可直接编译运行并复现实验结果；另附3个txt数据/说明文档与7张png结果图，方便对照分析。工程涉及预处理、点云生成、坐标变换、配准融合及后处理等关键步骤，内容紧凑、结构清晰，便于研读与二次修改。

已有1888人浏览学习。借助配套源码、动态库与实验数据，学习者不仅能复现点云融合效果，还能掌握RGB-D数据的处理思路与常见调试方法，为后续三维重建、机器人导航或AR应用研究打下扎实基础。

1. 点云融合：三帧RGB-D数据拼出一张完整点云

做视觉SLAM课设或者三维重建毕设的同学，大概率会卡在同一个地方：明明单帧点云看着还行，多帧一拼就重影、断层、颜色错乱，怎么调都像“玄学”。这次要拆的作业13-practice_pointCloudFusion，就用三帧不连续的RGB-D图像，配合一个轨迹文件里的4x4变换矩阵，把三个视角下的彩色点云融合到同一坐标系里，得到一张更密、更完整的场景点云。它不算高深算法，但把“坐标系变换、反投影、融合策略、滤波器”整条链路走了一遍，适合正在做点云融合实验、多帧重建课设的人照着搭一套可复现的代码骨架。

2. 需求与技术选型：为什么用RGB-D加齐次变换矩阵做多帧融合

2.1 需求拆解：从三帧图像到一张完整点云

这个实验给的数据很直接：data目录下有三组RGB图像和对应的深度图，外加一个cameraTrajectory.txt轨迹文件。相机不是连续采集，而是从三个不同位置拍下了同一个场景，这意味着帧与帧之间没有连续的视频流关系，只有轨迹文件给出的相对位姿。

需求其实可以拆成三层：

1. 读入单帧RGB-D图像，用深度图和相机内参恢复出这一帧的三维点云，也就是“反投影”。
2. 把第二帧、第三帧的点云通过轨迹文件里的变换矩阵转换到第一帧的坐标系下，解决“各说各话”的坐标系问题。
3. 将三帧点云合到一起。因为三帧视角有重叠也有互补，融合后点云密度会高于单帧，同时能填补单帧的遮挡空洞。

这也是点云融合实验最常见的标准流程。理解了这三层，后面看代码就不会迷路。

2.2 技术选型：C++与slamBase库的取舍

项目源码用的是C++，没有直接依赖PCL，而是封装了一个libslambase_shared.so动态库。选C++的原因很现实：点云数据动辄几十万个三维点，Python版PCL在数据量上来后循环遍历深度图会非常慢，而C++配合Release编译能控制在可接受范围内。

slamBase.hpp这个头文件是这个实验的“黑匣子入口”，它对外提供了相机模型、点云结构体、内参读取接口。你不需要自己去实现相机模型，但必须搞清楚它内部的数据结构。这种设计放在课设里是很合理的：核心算法自己写，底层数据结构用现成的库，既不至于淹没在细节里，又能把融合主流程摸透。

2.3 数据准备：depth、rgb与cameraTrajectory.txt的内容格式

开始写代码前，先看数据长什么样。通常RGB-D相机输出两种图：

• 深度图：单通道16位PNG，每个像素值是相机到物体的距离，单位一般是毫米。直接拿像素值当米用是新手最容易踩的坑。
• RGB图：普通三通道彩色图，和深度图逐像素对齐。

轨迹文件存的则是每一帧时刻对应的相机位姿，一般按“时间戳 + 4x4矩阵的12个旋转平移元素”排列。这个实验给定了三帧的变换矩阵，省去了自己估计位姿的麻烦，但同时也要求你必须正确解析矩阵，否则后面全乱。

3. 总体设计：数据流、坐标系与融合策略

3.1 数据流：从深度图到融合点云的四步管线

整个融合过程是一条单向管道，每一级输出是下一级输入：

读取RGB-D图像 -> 反投影生成单帧点云 -> 按变换矩阵转到参考系 -> 合并三帧点云 -> 滤波后处理

参考系选择第一帧的相机坐标系，这是一个约定俗成的做法。轨迹文件里第二帧、第三帧的变换矩阵，表示的就是“从当前帧坐标系到第一帧坐标系”的变换。换句话说，第一帧是“世界系”，其它帧是“相机系”，矩阵做的是相机系到世界系的转换。

3.2 反投影：内参矩阵与像素到三维点的映射

反投影是点云生成的根基。给定深度图上一点(u, v)，深度值z，相机内参fx, fy, cx, cy，三维坐标的计算公式是：

x = (u - cx) * z / fx y = (v - cy) * z / fy z = z（深度值本身）

这是典型的小孔成像模型逆变换。实现时要注意两点：一是深度值单位换算，二是无效像素的处理，深度为0、或者超过相机量程的像素必须跳过，否则会在点云里产生大量飞点。

3.3 变换：4x4齐次矩阵的乘法

拿到单帧点云后，下一步就是把第二、三帧的点云左乘变换矩阵。每个三维点(x, y, z)要先补成齐次坐标(x, y, z, 1)，然后左乘4x4矩阵，得到变换后的坐标。矩阵的前三行三列是旋转，第四列前三个元素是平移，最后一行固定是0 0 0 1。

这里最容易搞反的是“左乘还是右乘”。正确顺序是：

变换后点 = 变换矩阵 * 原始点

如果写反了，结果会完全不对，而且很难一眼看出来，只能通过最终点云的形状判断。

3.4 融合与后处理：先到先得与体素滤波

三帧点云都转到同一坐标系后，融合策略直接决定结果质量。最简单的策略是“后到覆盖先到”或“叠加保留”。对这个实验的静态场景，叠加后直接取并集就可以接受，因为三帧视角重叠区域较多但基本没有动态物体。

真正影响效果的是后处理。深度相机引入的噪声会让点云表面有一层“毛刺”，通常用两种滤波器解决：

• 体素滤波：对空间中一个固定大小的小立方体（voxel）内的所有点取重心，用一个点替代，既降密度又平滑表面。
• 统计滤波：计算每个点到最近K个邻居的平均距离，离群距离超过阈值的点视为噪声剔除。

体素大小选0.01米比较稳妥，太小起不到降采样作用，太大直接把物体细节抹平了。

4. 关键实现：核心代码与参数设计

4.1 slamBase.hpp对外接口

先看slamBase.hpp暴露了什么。通常一个RGB-D相机模型类会包含内参、深度缩放因子、图像尺寸，还有读取参数文件的方法。点云类型则是标准的“位置+颜色”结构体：

// slamBase.hpp 核心接口示意 struct Camera { double fx, fy, cx, cy; // 相机内参 double depthScale; // 深度缩放，1000表示毫米转米 int width, height; // 图像分辨率 }; // 点云中的一个点：三维坐标 + RGB颜色（用float存储0~255） struct PointCloudPoint { float x, y, z; float r, g, b; };

实际项目里不用自己写这个文件，但要清楚每个参数的含义。depthScale是重点：如果深度图单位是毫米，拿到像素值后要除以1000才得到米。你不除，融合出来的点云会比真实场景大1000倍，一眼看过去整个场景全是稀稀拉拉的噪点。

4.2 反投影实现：点云生成函数

单帧点云生成是整个实验最核心的函数。以第一帧为例，遍历深度图每个像素，做反投影和颜色映射：

// 生成单帧点云：输入RGB图、深度图、相机参数，输出点云 pcl::PointCloud<PointCloudPoint>::Ptr generatePointCloud( const cv::Mat& rgb, const cv::Mat& depth, const Camera& cam) { pcl::PointCloud<PointCloudPoint>::Ptr cloud(new pcl::PointCloud<PointCloudPoint>); for (int v = 0; v < cam.height; v++) { for (int u = 0; u < cam.width; u++) { // 读取深度值，注意单位换算 float d = depth.at<unsigned short>(v, u) / cam.depthScale; if (d <= 0.1 || d >= 10.0) continue; // 跳过无效深度 // 反投影到三维空间 double x = (u - cam.cx) * d / cam.fx; double y = (v - cam.cy) * d / cam.fy; double z = d; // 颜色直接从RGB图取 cv::Vec3b color = rgb.at<cv::Vec3b>(v, u); cloud->push_back({float(x), float(y), float(z), float(color[2]), float(color[1]), float(color[0])}); } } return cloud; }

这段代码有两个关键参数需要调：深度有效范围0.1~10.0米，根据实验场景决定；cam.depthScale必须和深度图实际单位一致。如果你发现点云里有一些“漂浮”的孤立点，多半是深度范围没限制住。

4.3 多帧融合主流程：pointCloudFusion.cpp

主流程的骨架是：读轨迹矩阵，逐帧生成点云，变换到第一帧坐标系，合并所有点：

// pointCloudFusion.cpp 融合主流程 int main(int argc, char** argv) { // 1. 读取相机参数与轨迹文件 Camera cam = readCameraParams("config/slamBase.yml"); vector<Eigen::Matrix4f> transforms = loadTrajectory("data/cameraTrajectory.txt"); // 2. 生成第一帧点云，并作为参考系 pcl::PointCloud<PointCloudPoint>::Ptr globalCloud(new pcl::PointCloud<PointCloudPoint>); cv::Mat rgb1 = cv::imread("data/rgb/1.png"); cv::Mat depth1 = cv::imread("data/depth/1.png", cv::IMREAD_UNCHANGED); *globalCloud += *generatePointCloud(rgb1, depth1, cam); // 3. 处理第二、三帧：反投影后变换到第一帧坐标系 for (int i = 2; i <= 3; i++) { cv::Mat rgb = cv::imread("data/rgb/" + to_string(i) + ".png"); cv::Mat depth = cv::imread("data/depth/" + to_string(i) + ".png", cv::IMREAD_UNCHANGED); auto cloud = generatePointCloud(rgb, depth, cam); pcl::PointCloud<PointCloudPoint>::Ptr transformed(new pcl::PointCloud<PointCloudPoint>); pcl::transformPointCloud(*cloud, *transformed, transforms[i - 1]); *globalCloud += *transformed; // 叠加合并 } // 4. 体素滤波 + 统计滤波 pcl::VoxelGrid<PointCloudPoint> voxel; voxel.setLeafSize(0.01f, 0.01f, 0.01f); voxel.setInputCloud(globalCloud); voxel.filter(*globalCloud); // 5. 保存结果，用PCL可视化或写PLY文件 pcl::io::savePLYFile("result/fused.ply", *globalCloud); return 0; }

注意transformPointCloud接受的是Eigen::Matrix4f，如果你的轨迹解析结果不是这个类型，要先转换。另外一个容易出错的地方是PLC的点云相加操作符，确保两个点云字段完全一致，否则编译不过或者结果错乱。

4.4 CMakeLists.txt与动态库链接

实验提供了一个现成的libslambase_shared.so和对应的CMakeLists.txt。链接动态库的核心配置如下：

cmake_minimum_required(VERSION 3.10) project(pointCloudFusion) set(CMAKE_CXX_STANDARD 11) # 查找PCL和OpenCV find_package(PCL REQUIRED) find_package(OpenCV REQUIRED) include_directories(${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include) # 链接自带的动态库 link_directories(${PROJECT_SOURCE_DIR}/lib) add_executable(pointCloudFusion src/pointCloudFusion.cpp src/slamBase.cpp) target_link_libraries(pointCloudFusion slambase_shared ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

如果编译时报找不到.so文件，多半是link_directories路径不对，或者运行时的LD_LIBRARY_PATH没有包含lib目录。用export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(pwd)/lib可以解决，但更好的做法是在CMake里加一句set(CMAKE_BUILD_RPATH_USE_ORIGIN ON)，把库路径写进可执行文件，省得每次都要手动设环境变量。

5. 常见问题与排查：融合翻车点与解决思路

5.1 融合后点云出现大片黑色空洞

现象：第三帧点云变换过来后，和第一帧重叠区域出现很多黑色缝隙，像是场景被“啃”了一块。

原因：最常见的是RGB图和深度图没对齐，或者读取时用了cv::imread的默认方式，把16位深度图转换成了8位，导致深度值被截断，部分像素直接变成0被过滤掉。

解决：读深度图必须加cv::IMREAD_UNCHANGED参数，保持16位原始数据。另外检查一下三帧图像的尺寸是否一致，如果width和height对不上，像素索引都会错位。

5.2 点数不增反减，融合后比单帧还稀

现象：三帧融合完，点云密度肉眼可见地下降，甚至出现大片空白区域。

原因：大概率是体素滤波的leafSize设得太大。比如设成0.05米，一个5厘米立方体里所有点都被合并成一个，如果原始点云本身精度在毫米级，这个降采样力度过于激进。

解决：leafSize从0.01米开始调，融合后点数应该约等于三帧点数之和除以一个较小的缩减比。也可以通过打印滤波前后点数来判断，如果从90万降到5万，说明体素格子太大了。

5.3 颜色错乱或物体边缘出现彩色“光晕”

现象：物体的边缘轮廓有红绿色边，墙面颜色不均匀，看起来像是染色失败。

原因：RGB图像和深度图像素没有精确对齐，导致深度图上属于物体A的区域，在RGB图上取到了物体B的颜色。这个在消费级RGB-D相机上非常常见，属于硬件标定问题。

解决：代码层面可以先对深度图做一次中值滤波，能轻微改善边缘噪声。如果还不行，检查生成点云时rgb.at<cv::Vec3b>(v,u)的索引顺序，OpenCV的通道顺序是BGR，不是RGB，直接存会出现红蓝互换。

5.4 变换矩阵写反，点云像“爆炸”一样散开

现象：融合后的场景分成了好几块，彼此错位严重，看起来像是点云被撕开了。

原因：变换矩阵方向搞反了。轨迹里给的是“当前帧到参考系”的矩阵，如果你用的是矩阵的逆，或者用成了“参考系到当前帧”，结果就会把所有点打到完全错误的位置。

解决：先验证一个简单情况：把第二帧的变换矩阵作用在第一帧原点(0,0,0)上，看结果是否落在第二帧相机应该出现的位置。如果方向反了，结果会相差一个旋转角度，一眼就能看出来。也可以在可视化时把三帧点云用不同颜色分别显示，重叠区域颜色错乱程度一目了然。

5.5 编译报错找不到libslambase_shared.so

现象：cmake . && make时报cannot find -lslambase_shared，或者运行时提示error while loading shared libraries。

原因：link_directories没有正确指向lib目录，或者动态库的软链接缺失。更像的是系统里没有安装该库到标准路径，而CMake只会在指定目录搜索。

解决：确认lib目录下文件存在，并且CMakeLists.txt里的link_directories(${PROJECT_SOURCE_DIR}/lib)写在了add_executable之前。运行时问题用LD_LIBRARY_PATH临时解决，或者在CMake里加上set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH} ${PROJECT_SOURCE_DIR}/lib")把库路径固化到可执行文件里。

6. 验收与答辩：量化指标与演示套路

6.1 验收标准：从数量和视觉两个维度打分

一份能拿得出手的融合实验，验收不能只靠“看着差不多”。通常我会从定量和定性两个维度各自列指标：

实验报告里建议放三张图：单帧点云截图、未滤波的融合点云、滤波后的最终结果。截图可以挑选有明显立体结构的区域，比如墙角和桌面边缘，这样融合增益效果最直观。

6.2 答辩技巧：把“变换矩阵”和“ICP配准”的关系讲清楚

答辩时被问得最多的一个问题是：为什么不直接用ICP做配准？这个问题的标准回答思路是：本实验给定了轨迹文件中的位姿，直接用变换矩阵完成坐标系统一，省去了ICP估计的过程，这样能把注意力集中在点云融合本身的步骤上。ICP在这个实验里只适合做微小误差的修正，如果初始位姿差得远，ICP很容易收敛到局部最优，结果比不配还差。

另一个加分项是主动解释“为什么选择第一帧做参考系”。你可以说：第一帧的相机坐标系作为世界系是一种惯例，所有帧都向它对齐，便于直观理解相机运动轨迹，也方便后续做增量式重建时扩展。

6.3 演示时的操作细节：让可视化成加分项

打开PCL可视化窗口时，点云会整体显示成一个灰色色块，需要手动加一个颜色处理。pcl::visualization::PointCloudColorHandlerRGBField能按点云自带颜色着色，如果你保存的PLY文件颜色字段正确，可视化代码只需要三行就能把它正常显示出来。演示时转动视角的节奏也值得注意：先展示整体场景，再缓慢旋转到三帧重叠区域，最后放大到细节处，让验收的人直观看到融合后的密度提升。

从那以后，我每次做多帧融合实验都会强制自己先跑一遍单帧点云生成和点数统计，确认第一帧看起来正常了再写融合逻辑。这一步看起来慢，实际能省掉后面排查变换矩阵错误的大部分时间。点云融合做到位了，再往深走就是网格重建和纹理映射，整个三维重建的管线就顺下来了，希望帮到你。

本文还有配套的精品资源，点击获取