How to extract NARF Features from a range image

This tutorial demonstrates how to extract NARF descriptors at NARF keypoint positions 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 then calculate the descriptors at these positions. It then visualizes these positions, both in an image and a 3D viewer.

The code

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

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 extract NARF keypoints from it. All of these steps are already covered in the previous tutorial NARF keypoint extraction.

The interesting part begins here:

...
std::vector<int> keypoint_indices2;
keypoint_indices2.resize(keypoint_indices.points.size());
for (unsigned int i=0; i<keypoint_indices.size(); ++i) // This step is necessary to get the right vector type
  keypoint_indices2[i]=keypoint_indices.points[i];
...

Here we copy the indices to the vector used as input for the feature.

...
pcl::NarfDescriptor narf_descriptor(&range_image, &keypoint_indices2);
narf_descriptor.getParameters().support_size = support_size;
narf_descriptor.getParameters().rotation_invariant = rotation_invariant;
pcl::PointCloud<pcl::Narf36> narf_descriptors;
narf_descriptor.compute(narf_descriptors);
std::cout << "Extracted "<<narf_descriptors.size()<<" descriptors for "<<keypoint_indices.points.size()<< " keypoints.\n";
...

This code does the actual calculation of the descriptors. It first creates the NarfDescriptor object and gives it the input data (the keypoint indices and the range image). Then two important parameters are set. The support size, which determines the size of the area from which the descriptor is calculated, and if the rotational invariant (rotation around the normal) version of the NARF descriptor should be used. The we create the output pointcloud and do the actual computation. At last, we output the number of keypoints and the number of extracted descriptors. This numbers can differ. For one, it might happen that the calculation of the descriptor fails, because there are not enough points in the range image (resolution too low). Or there might be multiple descriptors in the same place, but for different dominant rotations.

The resulting PointCloud contains the type Narf36 (see common/include/pcl/point_types.h) and store the descriptor as a 36 elements float and x,y,z,roll,pitch,yaw to describe the local coordinate frame at which the feature was extracted. The descriptors can now be compared, e.g., with the Manhattan distance (sum of absolute differences).

The remaining code just visualizes the keypoint positions 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_feature_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_feature_extraction narf_feature_extraction.cpp)
12target_link_libraries (narf_feature_extraction ${PCL_LIBRARIES})

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

$ ./narf_feature_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_feature_extraction <point_cloud.pcd>

The output should look similar to this:

_images/narf_keypoint_extraction.png