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