使用OpenCV4和PCL1.9实现深度图像转点云图像

一、参考高翔博士的一起做RGB-D SLAM (2)，针对在高版本环境下编译出现错误，做以下两点修改

1、读入图像路径

  rgb =cv::imread("./data/rgb.png");
  depth =cv::imread("./data/depth.png", -1);
  修改为
  rgb =cv::imread("../data/rgb.png");
  depth =cv::imread("../data/depth.png", -1);

2、针对在gcc 5.4.0版本下编辑不通过，设置编辑环境为gcc-7

SET(CMAKE_C_COMPILER "/usr/bin/gcc-7")
SET(CMAKE_CXX_COMPILER "/usr/bin/g++-7")

二、深度图像转点云图像源码

//图像到点云的转换

//C++ 标准库
#include <iostream>
#include <string>
using namespace std;

//OpenCV 库
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

//PCL 库
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

//相机内参
const double camera_factor = 1000;   //定义一个double类型的常变量（它的值不准修改）
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)
{
  // 读取./data/rgb.png和./data/depth.png，并转化为点云
  
  // 图像矩阵
  cv::Mat rgb, depth;
  // rgb 图像是8UC3的彩色图像
  rgb =cv::imread("../data/rgb.png");
  // 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)处的值
      //cv::Mat的ptr函数会返回指向该图像第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( "../result/pointcloud.pcd", *cloud );
    // 清除数据并退出
    cloud->points.clear();
    cout<<"Point cloud saved."<<endl;
    return 0;
}

三、CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
set(CMAKE_CXX_STANDARD 14)

SET(CMAKE_C_COMPILER "/usr/bin/gcc-7")
SET(CMAKE_CXX_COMPILER "/usr/bin/g++-7")

# 增加PCL库的依赖
#FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )
# Find Packages
find_package( PCL 1.9 REQUIRED )

# 增加opencv的依赖
FIND_PACKAGE( OpenCV REQUIRED )

# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )


ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp )
TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS} 
    ${PCL_LIBRARIES} )

四、结果展示