How to extract NARF keypoint from a range image

This tutorial demonstrates how to extract NARF key points from a range image. The executable enables us to load a point cloud from disc (or create it if not given), extract interest points on it and visualize the result, both in an image and a 3D viewer.

The code

First, create a file called, let’s say, narf_keypoint_extraction.cpp in your favorite editor, and place the following code inside it:

  1/* \author Bastian Steder */
  2
  3#include <iostream>
  4
  5#include <pcl/range_image/range_image.h>
  6#include <pcl/io/pcd_io.h>
  7#include <pcl/visualization/range_image_visualizer.h>
  8#include <pcl/visualization/pcl_visualizer.h>
  9#include <pcl/features/range_image_border_extractor.h>
 10#include <pcl/keypoints/narf_keypoint.h>
 11#include <pcl/console/parse.h>
 12#include <pcl/common/file_io.h> // for getFilenameWithoutExtension
 13
 14typedef pcl::PointXYZ PointType;
 15
 16// --------------------
 17// -----Parameters-----
 18// --------------------
 19float angular_resolution = 0.5f;
 20float support_size = 0.2f;
 21pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
 22bool setUnseenToMaxRange = false;
 23
 24// --------------
 25// -----Help-----
 26// --------------
 27void 
 28printUsage (const char* progName)
 29{
 30  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
 31            << "Options:\n"
 32            << "-------------------------------------------\n"
 33            << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
 34            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
 35            << "-m           Treat all unseen points as maximum range readings\n"
 36            << "-s <float>   support size for the interest points (diameter of the used sphere - "
 37            <<                                                     "default "<<support_size<<")\n"
 38            << "-h           this help\n"
 39            << "\n\n";
 40}
 41
 42//void 
 43//setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
 44//{
 45  //Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
 46  //Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
 47  //Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
 48  //viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
 49                            //look_at_vector[0], look_at_vector[1], look_at_vector[2],
 50                            //up_vector[0], up_vector[1], up_vector[2]);
 51//}
 52
 53// --------------
 54// -----Main-----
 55// --------------
 56int 
 57main (int argc, char** argv)
 58{
 59  // --------------------------------------
 60  // -----Parse Command Line Arguments-----
 61  // --------------------------------------
 62  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
 63  {
 64    printUsage (argv[0]);
 65    return 0;
 66  }
 67  if (pcl::console::find_argument (argc, argv, "-m") >= 0)
 68  {
 69    setUnseenToMaxRange = true;
 70    std::cout << "Setting unseen values in range image to maximum range readings.\n";
 71  }
 72  int tmp_coordinate_frame;
 73  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
 74  {
 75    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
 76    std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
 77  }
 78  if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
 79    std::cout << "Setting support size to "<<support_size<<".\n";
 80  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
 81    std::cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
 82  angular_resolution = pcl::deg2rad (angular_resolution);
 83  
 84  // ------------------------------------------------------------------
 85  // -----Read pcd file or create example point cloud if not given-----
 86  // ------------------------------------------------------------------
 87  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
 88  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
 89  pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
 90  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
 91  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
 92  if (!pcd_filename_indices.empty ())
 93  {
 94    std::string filename = argv[pcd_filename_indices[0]];
 95    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
 96    {
 97      std::cerr << "Was not able to open file \""<<filename<<"\".\n";
 98      printUsage (argv[0]);
 99      return 0;
100    }
101    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
102                                                               point_cloud.sensor_origin_[1],
103                                                               point_cloud.sensor_origin_[2])) *
104                        Eigen::Affine3f (point_cloud.sensor_orientation_);
105    std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
106    if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
107      std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
108  }
109  else
110  {
111    setUnseenToMaxRange = true;
112    std::cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
113    for (float x=-0.5f; x<=0.5f; x+=0.01f)
114    {
115      for (float y=-0.5f; y<=0.5f; y+=0.01f)
116      {
117        PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
118        point_cloud.push_back (point);
119      }
120    }
121    point_cloud.width = point_cloud.size ();  point_cloud.height = 1;
122  }
123  
124  // -----------------------------------------------
125  // -----Create RangeImage from the PointCloud-----
126  // -----------------------------------------------
127  float noise_level = 0.0;
128  float min_range = 0.0f;
129  int border_size = 1;
130  pcl::RangeImage::Ptr range_image_ptr (new pcl::RangeImage);
131  pcl::RangeImage& range_image = *range_image_ptr;   
132  range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
133                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
134  range_image.integrateFarRanges (far_ranges);
135  if (setUnseenToMaxRange)
136    range_image.setUnseenToMaxRange ();
137  
138  // --------------------------------------------
139  // -----Open 3D viewer and add point cloud-----
140  // --------------------------------------------
141  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
142  viewer.setBackgroundColor (1, 1, 1);
143  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
144  viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
145  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
146  //viewer.addCoordinateSystem (1.0f, "global");
147  //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
148  //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
149  viewer.initCameraParameters ();
150  //setViewerPose (viewer, range_image.getTransformationToWorldSystem ());
151  
152  // --------------------------
153  // -----Show range image-----
154  // --------------------------
155  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
156  range_image_widget.showRangeImage (range_image);
157  
158  // --------------------------------
159  // -----Extract NARF keypoints-----
160  // --------------------------------
161  pcl::RangeImageBorderExtractor range_image_border_extractor;
162  pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);
163  narf_keypoint_detector.setRangeImage (&range_image);
164  narf_keypoint_detector.getParameters ().support_size = support_size;
165  //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
166  //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
167  
168  pcl::PointCloud<int> keypoint_indices;
169  narf_keypoint_detector.compute (keypoint_indices);
170  std::cout << "Found "<<keypoint_indices.size ()<<" key points.\n";
171
172  // ----------------------------------------------
173  // -----Show keypoints in range image widget-----
174  // ----------------------------------------------
175  //for (std::size_t i=0; i<keypoint_indices.size (); ++i)
176    //range_image_widget.markPoint (keypoint_indices[i]%range_image.width,
177                                  //keypoint_indices[i]/range_image.width);
178  
179  // -------------------------------------
180  // -----Show keypoints in 3D viewer-----
181  // -------------------------------------
182  pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
183  pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
184  keypoints.resize (keypoint_indices.size ());
185  for (std::size_t i=0; i<keypoint_indices.size (); ++i)
186    keypoints[i].getVector3fMap () = range_image[keypoint_indices[i]].getVector3fMap ();
187
188  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
189  viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
190  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
191  
192  //--------------------
193  // -----Main loop-----
194  //--------------------
195  while (!viewer.wasStopped ())
196  {
197    range_image_widget.spinOnce ();  // process GUI events
198    viewer.spinOnce ();
199    pcl_sleep(0.01);
200  }
201}

Explanation

In the beginning we do command line parsing, read a point cloud from disc (or create it if not provided), create a range image and visualize it. All of these steps are already covered in the previous tutorial Range image visualization .

The interesting part begins here:

...
pcl::RangeImageBorderExtractor range_image_border_extractor;
pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);
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);
std::cout << "Found "<<keypoint_indices.size ()<<" key points.\n";
...

This creates a RangeImageBorderExtractor object, that is needed for the interest point extraction. If you are interested in this you can have a look at the Range Image Border Extraction tutorial. In this case we just use the RangeImageBorderExtractor object with its default parameters. Then we create the NarfKeypoint object, give it the RangeImageBorderExtractor object, the range image and set the support size (the size of the sphere around a point that includes points that are used for the determination of the interest value). The commented out part contains some parameters that you can test out if you want. Next we create the object where the indices of the determined keypoints will be saved and compute them. In the last step we output the number of found keypoints.

The remaining code just visualizes the results in a range image widget and also in a 3D viewer.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(narf_keypoint_extraction)
 4
 5find_package(PCL 1.3 REQUIRED)
 6
 7include_directories(${PCL_INCLUDE_DIRS})
 8link_directories(${PCL_LIBRARY_DIRS})
 9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (narf_keypoint_extraction narf_keypoint_extraction.cpp)
12target_link_libraries (narf_keypoint_extraction ${PCL_LIBRARIES})

After you have made the executable, you can run it. Simply do:

$ ./narf_keypoint_extraction -m

This will use an autogenerated point cloud of a rectangle floating in space. The key points are detected in the corners. The parameter -m is necessary, since the area around the rectangle is unseen and therefore the system can not detect it as a border. The option -m changes the unseen area to maximum range readings, thereby enabling the system to use these borders.

You can also try it with a point cloud file from your hard drive:

$ ./narf_keypoint_extraction <point_cloud.pcd>

The output should look similar to this:

_images/narf_keypoint_extraction.png