【PCL】Ubuntu20.04下 PCL深度图转化及其可视化

最新推荐文章于 2023-12-11 13:12:35 发布

野生技术白

最新推荐文章于 2023-12-11 13:12:35 发布

阅读量394

点赞数

分类专栏： PCL 文章标签： c++

本文链接：https://blog.csdn.net/qq_44225139/article/details/129412818

版权

PCL 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

基本概念

PCL中深度图的数据类型是 pcl::RangeImage，但是不要被这个“Image"和最终可视化的结果所迷惑了，pcl::RangeImage 实际的格式也是点，只不过这个点附加了采样点到视点（可以看作观察者）的距离信息，其点的类型为 pcl::PointWithRange。
深度图的可视化，首先需要从普通的类似于PointXYZ的点云图进行转化。当一个人站在一个位置，向点云观察，每个点云个体到人眼的距离都是不同的，记录下这个距离，以及人的眼睛（视点）的相关位姿信息，再叠加点云原本的坐标以及坐标系，就得到了RangeImage。可视化就是，当我们站在“视点”，拍一张照片，用不同的颜色表示每个点云到“视点”的距离的远近，给这张照片的每一个像素上色。

完整代码

#include <iostream>

#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

typedef pcl::PointXYZ PointType;

// --------------------
// -----Parameters-----
// --------------------

float angular_resolution_x = 0.5f,
      angular_resolution_y = angular_resolution_x;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool live_update = false;

// --------------
// -----Help-----
// --------------

void printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-rx <float>  angular resolution in degrees (default "<<angular_resolution_x<<")\n"
            << "-ry <float>  angular resolution in degrees (default "<<angular_resolution_y<<")\n"
            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
            << "-l           live update - update the range image according to the selected view in the 3D viewer.\n"
            << "-h           this help\n"
            << "\n\n";
}

/* 点云查看器位姿默认设置 */
//传入参数为点云查看器，仿射变换矩阵
void setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose) 
{
  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);  // 相机位置坐标
  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;  // 视点方向向量
  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);  // 视口向上
  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],  // 相机位置
                            look_at_vector[0], look_at_vector[1], look_at_vector[2],  // 视点方向
                            up_vector[0], up_vector[1], up_vector[2]);  // 视口向上方向上的xyz分量
}

// --------------
// -----Main-----
// --------------

int main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------

  if (pcl::console::find_argument (argc, argv, "-h") >= 0)  //  返回为找到的参数的索引，如果参数未出现在列表中，则为 -1
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::find_argument (argc, argv, "-l") >= 0)
  {
    live_update = true;
    std::cout << "Live update is on.\n";
  }

  // 设置X方向角分辨率
  if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0)  // 返回为找到的参数名称的索引，如果参数名称未出现在列表中，则为 -1，有的话更改参数对应的值
    std::cout << "Setting angular resolution in x-direction to "<<angular_resolution_x<<"deg.\n";

  // 设置Y方向角分辨率
  if (pcl::console::parse (argc, argv, "-ry", angular_resolution_y) >= 0)
    std::cout << "Setting angular resolution in y-direction to "<<angular_resolution_y<<"deg.\n";

  // 设置坐标系
  int tmp_coordinate_frame;
  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
  {
    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
    std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
  }
  angular_resolution_x = pcl::deg2rad (angular_resolution_x);  // 转化为弧度制
  angular_resolution_y = pcl::deg2rad (angular_resolution_y);
  
  // ------------------------------------------------------------------
  // -----Read pcd file or create example point cloud if not given-----
  // ------------------------------------------------------------------

  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);  // 创建PointXYZ类型的点云对象
  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;  // 建立引用对象

  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());  // 创建转换矩阵对象

  // 加载文件
  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");  // 将后缀为pcd的参数所在向量作为点云文件索引
  if (!pcd_filename_indices.empty ())  // 如果向量存在
  {
    std::string filename = argv[pcd_filename_indices[0]];  // 将该参数作为点云文件地址名
    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)  // 加载文件
    {
      std::cout << "Was not able to open file \""<<filename<<"\".\n";
      printUsage (argv[0]);
      return 0;
    }
    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
                                                             point_cloud.sensor_origin_[1],
                                                             point_cloud.sensor_origin_[2])) *  // 传感器位姿的平移信息
                        Eigen::Affine3f (point_cloud.sensor_orientation_);  // 传感器位姿的旋转信息  
  }
  else
  // 创建点云文件
  {
    std::cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
    for (float x=-0.5f; x<=0.5f; x+=0.01f)
    {
      for (float y=-0.5f; y<=0.5f; y+=0.01f)
      {
        PointType point;  
        point.x = x;  
        point.y = y;  
        point.z = 2.0f - y;
        point_cloud.push_back (point);
      }
    }
    point_cloud.width = point_cloud.size ();  
    point_cloud.height = 1;
  }
  
  // -----------------------------------------------
  // -----Create RangeImage from the PointCloud-----
  // -----------------------------------------------
  float noise_level = 0.0;
  float min_range = 0.0f;
  int border_size = 1;
  pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);  // 创建深度图对象，注意，深度图也是点云文件，只不过包含了视点到采样点的距离信息
  pcl::RangeImage& range_image = *range_image_ptr;   
  // 根据读取或者创建的点云文件来生成初始深度图()
  range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
                                    pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                    scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
  
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------

  pcl::visualization::PCLVisualizer viewer ("3D Viewer");  // 创建点云查看器
  viewer.setBackgroundColor (1, 1, 1);  // 设置点云查看器的背景颜色

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);  // 设置 range_image_ptr 深度图颜色的回调函数，此处整个点云对象为同一个颜色。点的类型为PointWithRange，附加了视点到采样点的距离信息
  viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");  // 将id为 range image 的深度图 range_image_ptr，根据颜色处理函数添加到屏幕
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");  // 设置点云渲染属性

  //viewer.addCoordinateSystem (1.0f, "global");
  //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
  //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");

  viewer.initCameraParameters ();  // 使用默认参数初始化点云查看器相机参数
  setViewerPose(viewer, range_image.getTransformationToWorldSystem ());  // 设置点云查看器位姿（从深度图像坐标系转换到世界坐标系）
  
  // --------------------------
  // -----Show range image-----
  // --------------------------

  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");  // 创建深度图查看器
  range_image_widget.showRangeImage (range_image);  // 可视化深度图
  // pcl_sleep (1);  // 休眠（等待） 0.01 秒
  
  //--------------------
  // -----Main loop-----
  //--------------------

  while (!viewer.wasStopped ())
  {
    range_image_widget.spinOnce ();  // 展示深度图（只一次）
    viewer.spinOnce ();  // 展示点云（只一次）
    pcl_sleep (0.01);  // 休眠 0.01 秒
    
    if (live_update)  // 不断更新
    {
      scene_sensor_pose = viewer.getViewerPose();  // 得到当前点云观看位姿
      range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
                                        pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                        scene_sensor_pose, pcl::RangeImage::CAMERA_FRAME, noise_level, min_range, border_size);  // 根据当前点云实时创建深度图
      range_image_widget.showRangeImage (range_image);  // 刷新显示深度图
    }
  }
}

代码主要来自PCL的官方文档，本人添加了备注以及做了少许修改

分段解读

默认参数的设定

设置了深度图可视化的相关参数，分别是
xy方向的角分辨率
坐标系类型
是否根据点云图实时更新深度图

float angular_resolution_x = 0.5f,
      angular_resolution_y = angular_resolution_x;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool live_update = false;

使用说明的编写

这一段是可执行文件的相关使用参数编写

void printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-rx <float>  angular resolution in degrees (default "<<angular_resolution_x<<")\n"
            << "-ry <float>  angular resolution in degrees (default "<<angular_resolution_y<<")\n"
            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
            << "-l           live update - update the range image according to the selected view in the 3D viewer.\n"
            << "-h           this help\n"
            << "\n\n";
}

可视化查看器位姿设定

这一段代码主要的功能是根据获得的点云文件的视点信息，进一步设置可视化查看器的视点信息。

void setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose) 
{
  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);  // 相机位置坐标
  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;  // 视点方向向量
  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);  // 视口向上方向分量
  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],  // 相机位置
                            look_at_vector[0], look_at_vector[1], look_at_vector[2],  // 视点方向
                            up_vector[0], up_vector[1], up_vector[2]);  // 视口向上方向上的xyz分量
}

主函数中关于参数的识别和配置代码

这一段根据输入的可执行文件参数，设置可视化的相关设定，从而对默认参数进行修改，以及提供所要查看的点云文件的地址。

if (pcl::console::find_argument (argc, argv, "-h") >= 0)  //  返回为找到的参数的索引，如果参数未出现在列表中，则为 -1
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::find_argument (argc, argv, "-l") >= 0)
  {
    live_update = true;
    std::cout << "Live update is on.\n";
  }

  // 设置X方向角分辨率
  if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0)  // 返回为找到的参数名称的索引，如果参数名称未出现在列表中，则为 -1，有的话更改参数对应的值
    std::cout << "Setting angular resolution in x-direction to "<<angular_resolution_x<<"deg.\n";

  // 设置Y方向角分辨率
  if (pcl::console::parse (argc, argv, "-ry", angular_resolution_y) >= 0)
    std::cout << "Setting angular resolution in y-direction to "<<angular_resolution_y<<"deg.\n";

  // 设置坐标系
  int tmp_coordinate_frame;
  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
  {
    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
    std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
  }
  angular_resolution_x = pcl::deg2rad (angular_resolution_x);  // 转化为弧度制
  angular_resolution_y = pcl::deg2rad (angular_resolution_y);

读取文件

这里对指定地址的文件进行读取，若读取成功，则向 scene_sensor_pose 变量内写入，根据点云文件获取的原本的“视点”信息换算得到的仿射矩阵。从而获得点云和世界坐标系的关系。

  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);  // 创建PointXYZ类型的点云对象
  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;  // 建立引用对象

  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());  // 创建转换矩阵对象

  // 加载文件
  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");  // 将后缀为pcd的参数所在向量作为点云文件索引
  if (!pcd_filename_indices.empty ())  // 如果向量存在
  {
    std::string filename = argv[pcd_filename_indices[0]];  // 将该参数作为点云文件地址名
    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)  // 加载文件
    {
      std::cout << "Was not able to open file \""<<filename<<"\".\n";
      printUsage (argv[0]);
      return 0;
    }
    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
                                                             point_cloud.sensor_origin_[1],
                                                             point_cloud.sensor_origin_[2])) *  // 传感器位姿的平移信息
                        Eigen::Affine3f (point_cloud.sensor_orientation_);  // 传感器位姿的旋转信息  
  }

读取失败则创建点云文件

else
  // 创建点云文件
  {
    std::cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
    for (float x=-0.5f; x<=0.5f; x+=0.01f)
    {
      for (float y=-0.5f; y<=0.5f; y+=0.01f)
      {
        PointType point;  
        point.x = x;  
        point.y = y;  
        point.z = 2.0f - y;
        point_cloud.push_back (point);
      }
    }
    point_cloud.width = point_cloud.size ();  
    point_cloud.height = 1;
  }

转换得到深度图

我们需要根据获取的点云文件或者自己创建的点云文件来生成深度图，一开始是深度图可视化的一些参数设定，包含
噪声水平，
最小范围，
边框宽度。
之后创建了一个深度图对象，用于容纳深度图，并创建了一个引用对象。
最后一段代码是根据点云文件创造深度图，参数包含了
点云文件
x、y方向的分辨率
X、Y方向的最大可视范围
视点位姿信息
坐标系类型
噪声水平
最小可见范围（深度小于这个的会被忽略）
边框宽度
返回的就是一个深度图了

  float noise_level = 0.0;
  float min_range = 0.0f;
  int border_size = 1;
  pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);  // 创建深度图对象，注意，深度图也是点云文件，只不过包含了视点到采样点的距离信息
  pcl::RangeImage& range_image = *range_image_ptr;   
  // 根据读取或者创建的点云文件来生成初始深度图()
  range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
                                    pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                    scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);

创建3D查看器

该段代码创建了一个 PCLVisualizer 查看器，其与 cloudviewer 相比功能更加强大，当然也需要更多的设置。
最后一段是“视点”的设置，获得查看器的位姿信息。

  pcl::visualization::PCLVisualizer viewer ("3D Viewer");  // 创建点云查看器
  viewer.setBackgroundColor (1, 1, 1);  // 设置点云查看器的背景颜色

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);  // 设置 range_image_ptr 深度图颜色的回调函数，此处整个点云对象为同一个颜色。点的类型为PointWithRange，附加了视点到采样点的距离信息
  viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");  // 将id为 range image 的深度图 range_image_ptr，根据颜色处理函数添加到屏幕
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");  // 设置点云渲染属性

  viewer.initCameraParameters ();  // 使用默认参数初始化点云查看器相机参数
  setViewerPose(viewer, range_image.getTransformationToWorldSystem ());  // 设置点云查看器位姿（从深度图像坐标系转换到世界坐标系）

简单显示深度图

这一段可以进行深度图的简单展示

  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");  // 创建深度图查看器
  range_image_widget.showRangeImage (range_image);  // 可视化深度图
  // pcl_sleep (1);  // 休眠（等待） 0.01 秒

深度图可视化

当我们未选择持续更新时，深度图会连同点云模型一起显示，但是调整点云模型，深度图不会发生变化，
当我们选择持续更新，深度图会随着点云模型的位姿变化不断更新。

  while (!viewer.wasStopped ())
  {
    range_image_widget.spinOnce ();  // 展示深度图（只一次）
    viewer.spinOnce ();  // 展示点云（只一次）
    pcl_sleep (0.01);  // 休眠 0.01 秒
    
    if (live_update)  // 不断更新
    {
      scene_sensor_pose = viewer.getViewerPose();  // 得到当前点云观看位姿
      range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
                                        pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                        scene_sensor_pose, pcl::RangeImage::CAMERA_FRAME, noise_level, min_range, border_size);  // 根据当前点云实时创建深度图
      range_image_widget.showRangeImage (range_image);  // 刷新显示深度图
    }
  }

CMake代码

注意：源文件位于src文件夹下

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(range_image_visualization)

find_package(PCL 1.3 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (range_image_visualization src/range_image_visualization.cpp)
target_link_libraries (range_image_visualization ${PCL_LIBRARIES})