PCL系列——从深度图像(RangeImage)中提取NARF关键点

PCL系列

说明

通过本教程，我们将会学会：如何从深度图像中提取NARF关键点。
首先从硬盘中读取点云文件，然后提取它的NARF关键点，最后显示结果。
下面的代码中，先是命令行解析，然后是读取点云文件，如果点云文件不存在，就创造一个深度图像，并显示它。
操作

参照之前的文章，配置项目的属性。设置包含目录和库目录和附加依赖项。
在VS2010 中新建一个文件 narf_keypoint_extraction.cpp，然后将下面的代码复制到文件中。
#include <iostream> //标准输入输出流
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
#include <pcl/console/parse.h>

typedef pcl::PointXYZ PointType; //定义别名

//参数 全局变量
float angular_resolution = 0.5f; //角坐标分辨率
float support_size = 0.2f; //感兴趣点的尺寸（球面的直径）
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; //坐标框架：相机框架（而不是激光框架）
bool setUnseenToMaxRange = false; //是否将所有不可见的点 看作 最大距离


//帮助
//当用户输入命令行参数-h，打印帮助信息
void printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
            << "-m           Treat all unseen points as maximum range readings\n"
            << "-s <float>   support size for the interest points (diameter of the used sphere - "
            <<                                                     "default "<<support_size<<")\n"
            << "-h           this help\n"
            << "\n\n";
}


int main (int argc, char** argv)
{

  //解析 命令行 参数
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::find_argument (argc, argv, "-m") >= 0)
  {
    setUnseenToMaxRange = true;
    cout << "Setting unseen values in range image to maximum range readings.\n";
  }
  int tmp_coordinate_frame;
  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
  {
    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame); //以函数的方式初始化（0：相机框架；1：激光框架）
    cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
  }
  if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
    cout << "Setting support size to "<<support_size<<".\n";
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
  angular_resolution = pcl::deg2rad (angular_resolution);
  
  //读取pcd文件；如果没有指定文件，就创建样本点
  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>); //点云指针
  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr; //上面点云的别名
  pcl::PointCloud<pcl::PointWithViewpoint> far_ranges; //带视角的点构成的点云
  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 ())//如果指定了pcd文件，读取pcd文件和对应的远距离pcd文件
  {
    std::string filename = argv[pcd_filename_indices[0]]; //文件名
    if (pcl::io::loadPCDFile (filename, point_cloud) == -1) //读取pcd文件
    {
      cerr << "Was not able to open file \""<<filename<<"\".\n"; //是否应该是std::cerr
      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_); //设置传感器的姿势
    std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd"; //远距离文件名
    if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1) //读取远距离pcd文件
      std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
  }
  else //没有指定pcd文件，生成点云，并填充它
  {
    setUnseenToMaxRange = true;
    cout << "\nNo *.pcd file given => Genarating 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.points.push_back (point); //设置点云中点的坐标
      }
    }
    point_cloud.width = (int) point_cloud.points.size ();  
		point_cloud.height = 1;
  }
  

  //从点云数据，创建深度图像
  float noise_level = 0.0;
  float min_range = 0.0f;
  int border_size = 1;
  boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage); //创建RangeImage对象（指针）
  pcl::RangeImage& range_image = *range_image_ptr;  //引用
  range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size); //从点云创建深度图像
  range_image.integrateFarRanges (far_ranges); //整合远距离点云
  if (setUnseenToMaxRange)
    range_image.setUnseenToMaxRange ();
  
  //打开3D观察图形窗口，并添加点云
  pcl::visualization::PCLVisualizer viewer ("3D Viewer"); //创建3D Viewer对象
  viewer.setBackgroundColor (1, 1, 1); //设置背景色
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
  viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image"); //添加点云
  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 ());
  
  //显示深度图像（平面图，上面的3D显示）
  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
  range_image_widget.showRangeImage (range_image);
  
  //提取NARF关键点
  pcl::RangeImageBorderExtractor range_image_border_extractor; //创建深度图像的边界提取器，用于提取NARF关键点
  pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor); //创建NARF对象
  narf_keypoint_detector.setRangeImage (&range_image);
  narf_keypoint_detector.getParameters ().support_size = support_size;
  //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
  //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
  
  pcl::PointCloud<int> keypoint_indices; //用于存储关键点的索引
  narf_keypoint_detector.compute (keypoint_indices); //计算NARF关键点
  std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";

  //在range_image_widget中显示关键点
  //for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    //range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
                                  //keypoint_indices.points[i]/range_image.width);
  

  //在3D图形窗口中显示关键点
  pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>); //创建关键点指针
  pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr; //引用
  keypoints.points.resize (keypoint_indices.points.size ()); //点云变形，无序
  for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
  viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
  
  // -----Main loop-----
  while (!viewer.wasStopped ())
  {
    range_image_widget.spinOnce ();  // 处理 GUI事件
    viewer.spinOnce ();
    pcl_sleep(0.01);
  }
}
重新生成项目。
到改项目的Debug目录下，按住Shift，同时点击鼠标右键，在当前窗口打开CMD窗口。
在命令行中输入narf_keypoint_extraction.exe执行程序，narf_keypoint_extraction.exe -h显示帮助信息。
参考

How to extract NARF keypoint from a range image