cmake_minimum_required( VERSION 2.8 )
project( joinMap )set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )# opencv
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )# eigen
include_directories( "/usr/include/eigen3/" )# pcl
find_package( PCL REQUIRED COMPONENT common io )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )add_executable( joinMap joinMap.cpp )
target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )
#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>int main( int argc, char** argv )
{vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses; // 相机位姿ifstream fin("./pose.txt");if (!fin){cerr<<"请在有pose.txt的目录下运行此程序"<<endl;return 1;}for ( int i=0; i<5; i++ ){boost::format fmt( "./%s/%d.%s" ); //图像文件格式colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像double data[7] = {0};for ( auto& d:data )fin>>d;Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );Eigen::Isometry3d T(q);T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));poses.push_back( T );}// 计算点云并拼接// 相机内参 double cx = 325.5;double cy = 253.5;double fx = 518.0;double fy = 519.0;double depthScale = 1000.0;cout<<"正在将图像转换为点云..."<<endl;// 定义点云使用的格式：这里用的是XYZRGBtypedef pcl::PointXYZRGB PointT; typedef pcl::PointCloud<PointT> PointCloud;// 新建一个点云PointCloud::Ptr pointCloud( new PointCloud ); for ( int i=0; i<5; i++ ){cout<<"转换图像中: "<<i+1<<endl; cv::Mat color = colorImgs[i]; cv::Mat depth = depthImgs[i];Eigen::Isometry3d T = poses[i];for ( int v=0; v<color.rows; v++ )for ( int u=0; u<color.cols; u++ ){unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值if ( d==0 ) continue; // 为0表示没有测量到Eigen::Vector3d point; point[2] = double(d)/depthScale; point[0] = (u-cx)*point[2]/fx;point[1] = (v-cy)*point[2]/fy; Eigen::Vector3d pointWorld = T*point;PointT p ;p.x = pointWorld[0];p.y = pointWorld[1];p.z = pointWorld[2];p.b = color.data[ v*color.step+u*color.channels() ];p.g = color.data[ v*color.step+u*color.channels()+1 ];p.r = color.data[ v*color.step+u*color.channels()+2 ];pointCloud->points.push_back( p );}}pointCloud->is_dense = false;cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;pcl::io::savePCDFileBinary("map.pcd", *pointCloud );return 0;
}这段代码是机器人视觉和 SLAM（即时定位与地图构建）中非常经典的程序：将多帧 RGB-D 图像根据相机位姿拼接成一个全局的三维点云地图。
它涉及了三个核心库：OpenCV（图像处理）、Eigen（几何变换）和 PCL（点云处理）。下面我为你逐模块详细拆解：
1. 数据准备与读取
vector<cv::Mat> colorImgs, depthImgs; // 存放多帧彩色图和深度图
vector<Eigen::Isometry3d, ...> poses; // 存放每帧图片对应的相机位姿（即相机在世界坐标系里的位置）ifstream fin("./pose.txt"); // 读取位姿文件Eigen::Isometry3d: 这表示“欧式变换矩阵”（），包含旋转和平移。- 循环读取 (0-4): 程序默认处理 5 组数据。
boost::format: 一个很方便的字符串格式化工具，用来生成文件名（如./color/1.png）。cv::imread(..., -1): 读取深度图时必须加-1（或IMREAD_UNCHANGED），否则 OpenCV 会把它当成普通彩色图，导致深度精度丢失。- 位姿转换: 从文本读取 7 个数（前 3 个是平移 ，后 4 个是四元数 ）。代码将其转化为
Eigen::Isometry3d变换矩阵T。
2. 相机内参 (Camera Intrinsics)
这是将二维像素点还原到三维空间的关键参数：
double cx = 325.5; double cy = 253.5; // 相机光心坐标
double fx = 518.0; double fy = 519.0; // 焦距
double depthScale = 1000.0; // 深度图缩放因子（通常 1000 表示 1mm 对应数值 1）- 这些参数通常是通过相机标定得到的。
3. PCL 点云定义
typedef pcl::PointXYZRGB PointT; // 定义点类型：包含坐标 (XYZ) 和颜色 (RGB)
typedef pcl::PointCloud<PointT> PointCloud; // 定义点云类型
PointCloud::Ptr pointCloud( new PointCloud ); // 像智能指针一样新建一个空点云4. 核心：从像素到三维点的转换 (投影模型)
这是程序最精华的部分，嵌套在双重循环（遍历图像行 v 和列 u）中：
第一步：获取深度
unsigned int d = depth.ptr<unsigned short> ( v )[u];
if ( d==0 ) continue; // 深度为0代表遮挡或距离太远，无法测量，跳过第二步：计算相机坐标系下的三维坐标
根据针孔相机模型公式：
Eigen::Vector3d point;
point[2] = double(d)/depthScale;
point[0] = (u-cx)*point[2]/fx;
point[1] = (v-cy)*point[2]/fy; - 此时的
point是相对于相机本身的坐标（以摄像头为原点）。
第三步：变换到世界坐标系
Eigen::Vector3d pointWorld = T * point;- 由于相机在运动，每一帧的
point都在不同位置。通过乘以位姿矩阵T，我们将所有点都统一变换到世界坐标系（统一地图）下。
第四步：赋予颜色并填充点云
p.x = pointWorld[0]; // 写入变换后的三维坐标
// ...
p.b = color.data[ v*color.step + u*color.channels() ]; // 获取像素颜色
// ...
pointCloud->points.push_back( p ); // 将点放入大地图中- 注意：OpenCV 默认颜色顺序是 BGR，所以下标
+1是 G，+2是 R。
5. 保存结果
pointCloud->is_dense = false; // 点云中可能存在无效点（虽然我们跳过了d=0，但习惯上设为false）
pcl::io::savePCDFileBinary("map.pcd", *pointCloud ); // 以二进制格式保存点云- 保存生成的
map.pcd文件后，你可以使用工具（如pcl_viewer map.pcd）在电脑上看到一个精美的一比一还原的三维空间场景。
总结：学会这个流程你需要记住的
- 读数据：彩色图取色，深度图取距离，位姿取位置。
- 算坐标：利用内参（）把 变成 。
- 拼地图：利用位姿 把每一帧的坐标转到同一个世界坐标系。
- 存文件：用 PCL 库把这些点聚在一起存成
.pcd。
这段代码是三维重建的“基石”，理解了它，你就迈进了 SLAM 算法的大门！