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

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.points.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 2.6 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