深度相机-生成点云图-23
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;
// 定义点云使用的格式：这里用的是XYZRGB
typedef 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 算法的大门！
浙公网安备 33010602011771号