ROS中点云学习（六）：3D激光点云投影在2D环视图上,得到深度图和强度图

主要思想：使用的点云为16行，每行1800个点，点云每个点自带ring，再根据xy可以求出来点在哪一列。然后把点的深度和强度信息传输给Mat的矩阵即可。

问题：因为使用的点云原因，得到的图像是很扁的，所以看起来并不好看。尝试使用opencv拉伸，但是失败了。

和上一篇类似，为了主程序的简洁，我们建立一个头文件myPointType.h，在这里面我们定义我们所需要的点云类型，具体代码如下：

#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE


#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

/*
    *一个具有XYZ、intensity、ring的点云类型
    */
struct PointXYZIR
{
    
    
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (uint16_t, ring, ring)
)


/*
    * 一个具有XYZ、RGB、intensity、ring的点云类型
    */
struct PointXYZRGBIR
{
    
    
    PCL_ADD_POINT4D;
    PCL_ADD_RGB;
    PCL_ADD_INTENSITY;
    uint16_t ring;
    uint16_t label;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBIR,  
                            (float, x, x)
                            (float, y, y)
                            (float, z, z)
                            (float, rgb, rgb)
                            (float, intensity, intensity)
                            (uint16_t, label, label)
                            (uint16_t, ring, ring)
)
 
#endif  //PCL_NO_PRECOMPILE

另外加一个参数的头文件pamarm.h，具体如下：

#ifndef PAMARM_NO_PRECOMPILE
#define PAMARM_NO_PRECOMPILE

#include <math.h>

extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2; //水平上每条线间隔0.2°
extern const float sensorMinimumRange = 1.0;

#endif  // PAMARM_NO_PRECOMPILE

主要程序为pcl_projection.cpp，具体代码如下：

// #include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "myPointType.h"
#include "pamarm.h"



class pcl_projection
{
    
    
private:
  ros::NodeHandle n;
  ros::Subscriber subCloud;
  cv::Mat rangeMat; //点云的range矩阵
  cv::Mat rangeImg; //点云的range图像
  cv::Mat intensityMat; //点云的反射强度矩阵，也是float类型
  cv::Mat intensityImg; //点云的反射强度矩阵

public:
  pcl_projection():
    n("~"){
    
    
      subCloud = n.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &pcl_projection::getcloud, this); 
      void resetParameters();
    }

  void resetParameters(){
    
         
    rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(111111));
    rangeImg = cv::Mat(N_SCAN, Horizon_SCAN, CV_8UC1, cv::Scalar(255));
    intensityMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(111111));
    intensityImg = cv::Mat(N_SCAN, Horizon_SCAN, CV_8UC1, cv::Scalar(255));
  }

  //点云回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    
    
    float x,y,z,horizonAngle, range, intensity; //xyz,水平角,距离
    size_t rowIdn, columnIdn, cloudSize;  //行索引,列索引,点云数量
    uint8_t range_int, intensity_int;

    pcl::PointCloud<PointXYZIR>::Ptr   raw_pcl_ptr (new pcl::PointCloud<PointXYZIR>);  
    pcl::fromROSMsg(*laserCloudMsg, *raw_pcl_ptr);  //把msg消息指针转化为点云指正
    cloudSize = raw_pcl_ptr->points.size();
    for (int i = 0; i <  cloudSize; i++)
    {
    
    
      x = raw_pcl_ptr->points[i].x;
      y = raw_pcl_ptr->points[i].y;
      z = raw_pcl_ptr->points[i].z;
      rowIdn = raw_pcl_ptr->points[i].ring;   //行索引
      intensity = raw_pcl_ptr->points[i].intensity;
      if (rowIdn < 0 || rowIdn >= N_SCAN)
        continue;
      horizonAngle = atan2(x, y) * 180 / M_PI;     //水平角
      columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;   //列索引
      if (columnIdn >= Horizon_SCAN)
        columnIdn -= Horizon_SCAN;
      if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
        continue;
      range = sqrt(x * x + y *y + z * z);  //距离
      if (range < sensorMinimumRange)
        continue;

      rangeMat.at<float>(rowIdn, columnIdn) = range;   //距离传递出去
      range_int = uint8_t(range*5); //使得距离尽量在0-255内。
      if (range_int > 255)
        range_int = 255;
      rangeImg.at<uint8_t>(rowIdn, columnIdn) = range_int;   //强度展示

      intensityMat.at<float>(rowIdn, columnIdn) = intensity;   //强度传递出去
      intensity_int = uint8_t(intensity*255/100);
      intensityImg.at<uint8_t>(rowIdn, columnIdn) = intensity_int;   //强度展示
    }

    cv::imshow("range", rangeImg);  //可视化
    cv::imshow("intensity", intensityImg);
  }

  ~pcl_projection(){
    
    };

};

int main(int argc, char **argv)
{
    
    
  ros::init(argc, argv, "pcl_projection");
  cv::namedWindow("range");  //两个窗口，分别可视化range和intensity
  cv::namedWindow("intensity");
  cv::startWindowThread();
  pcl_projection pp;
  pp.resetParameters();  //初始化参数
  ros::spin();
  cv::destroyWindow("range");
  cv::destroyWindow("intensity");
}

CMakeLists.txt文件为：

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(pcl_projection)

set(PACKAGE_DEPENDENCIES
  roscpp
  sensor_msgs
  pcl_ros
  pcl_conversions
  std_srvs
  message_generation 
  std_msgs
)

find_package(OpenCV REQUIRED)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(pcl_projection pcl_projection.cpp)
target_link_libraries(pcl_projection ${PCL_LIBRARIES}  ${catkin_LIBRARIES} ${OpenCV_LIBS})

最后得到的效果图：