RGB图像转换成3D点云图像

#include <iostream>
#include <string>
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
using namespace std;
using namespace cv;
// 定义点云类型
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 相机内参
const double camera_factor = 1000;
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;
int main( int argc, char** argv )
{
// 图像矩阵
cv::Mat rgb, depth;
// 使用cv::imread()来读取图像
rgb = cv::imread( "/data/color.png" );
// rgb 图像是8UC3的彩色图像
// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
depth = cv::imread( "/data/depth.png", -1 );
// 点云变量
// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
PointCloud::Ptr cloud ( new PointCloud );
// 遍历深度图
for (int m = 0; m < depth.rows; m++)
for (int n=0; n < depth.cols; n++)
{
// 获取深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d 可能没有值，若如此，跳过此点
if (d == 0)
continue;
// d 存在值，则向点云增加一个点
PointT p;
// 计算这个点的空间坐标
p.z = double(d) / camera_factor;
p.x = (n - camera_cx) * p.z / camera_fx;
p.y = (m - camera_cy) * p.z / camera_fy;
// 从rgb图像中获取它的颜色
// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
p.b = rgb.ptr<uchar>(m)[n*3];
p.g = rgb.ptr<uchar>(m)[n*3+1];
p.r = rgb.ptr<uchar>(m)[n*3+2];
// 把p加入到点云中
cloud->points.push_back( p );
}
// 设置并保存点云
cloud->height = 1;
cloud->width = cloud->points.size();
cout<<"point cloud size = "<<cloud->points.size()<<endl;
cloud->is_dense = false;
pcl::io::savePCDFile( "pointloud.pcd", *cloud );
// 清除数据并退出c
cloud->points.clear();
cout<<"Point cloud saved."<<endl;
return 0;
}

本文转自https://blog.csdn.net/leon_zeng0/article/details/72956750

对应的CMakelists,txt代码如下：

cmake_minimum_required(VERSION 2.6)
project(rgbtopoint)

add_executable(rgbtopoint main.cpp)

install(TARGETS rgbtopoint RUNTIME DESTINATION bin)

set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
#list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
# opencv 
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

# pcl 
find_package( PCL REQUIRED)
#find_package( PCL REQUIRED COMPONENT common io )
#FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization )
include_directories( ${PCL_INCLUDE_DIRS} )
link_directories(${PCL_LIBRARY_DIRS})
add_definitions( ${PCL_DEFINITIONS} )

target_link_libraries( rgbtopoint ${OpenCV_LIBS} ${PCL_LIBRARIES} )