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:
