PCL点特征描述与提取

1、NARF_Feature_Extraction

cpp

[html]  view plain  copy

 

1. /* \author Bastian Steder */  
2. #include <iostream>  
3. #include <boost/thread/thread.hpp>  
4. #include <pcl/range_image/range_image.h>  
5. #include <pcl/io/pcd_io.h>  
6. #include <pcl/visualization/range_image_visualizer.h>  
7. #include <pcl/visualization/pcl_visualizer.h>  
8. #include <pcl/features/range_image_border_extractor.h>  
9. #include <pcl/keypoints/narf_keypoint.h>  
10. #include <pcl/features/narf_descriptor.h>  
11. #include <pcl/console/parse.h>  
12.   
13. #include "vtkImageViewer.h"  
14. #include "vtkInteractionImageModule.h"  
15. #include "vtkInteractionWidgetsModule.h"  
16. typedef pcl::PointXYZ PointType;  
17. // -----参数-----  
18. // --------------------  
19. float angular_resolution = 0.5f;  
20. float support_size = 0.2f;  
21. pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;  
22. bool setUnseenToMaxRange = false;  
23. bool rotation_invariant = true;  
24.   
25. // -----帮助-----  
26. void   
27. printUsage (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 to max range\n"  
35.             << "-s <float>   support size for the interest points (diameter of the used sphere - "  
36.                                                                   "default "<<support_size<<")\n"  
37.             << "-o <0/1>     switch rotational invariant version of the feature on/off"  
38.             <<               " (default "<< (int)rotation_invariant<<")\n"  
39.             << "-h           this help\n"  
40.             << "\n\n";  
41. }  
42.   
43. //void   
44. //setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)  
45. //{  
46. //  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);  
47. //  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;  
48. //  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);  
49. //  viewer.camera_.pos[0] = pos_vector[0];  
50. //  viewer.camera_.pos[1] = pos_vector[1];  
51. //  viewer.camera_.pos[2] = pos_vector[2];  
52. //  viewer.camera_.focal[0] = look_at_vector[0];  
53. //  viewer.camera_.focal[1] = look_at_vector[1];  
54. //  viewer.camera_.focal[2] = look_at_vector[2];  
55. //  viewer.camera_.view[0] = up_vector[0];  
56. //  viewer.camera_.view[1] = up_vector[1];  
57. //  viewer.camera_.view[2] = up_vector[2];  
58. //  viewer.updateCamera ();  
59. //}  
60. // -----主函数-----  
61. // --------------  
62. int   
63. main (int argc, char** argv)  
64. {  
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.     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.     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.     cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";  
83.   }  
84.   if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)  
85.     cout << "Setting support size to "<<support_size<<".\n";  
86.   if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)  
87.     cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";  
88.   angular_resolution = pcl::deg2rad (angular_resolution);  
89. // -----读取pcd文件，或者如果没有给出，则创建一个样本点云-----  
90.   pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);  
91.   pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;  
92.   pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;  
93.   Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());  
94.   std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");  
95.   if (!pcd_filename_indices.empty ())  
96.   {  
97.     std::string filename = argv[pcd_filename_indices[0]];  
98.     if (pcl::io::loadPCDFile (filename, point_cloud) == -1)  
99.     {  
100.       cerr << "Was not able to open file \""<<filename<<"\".\n";  
101.       printUsage (argv[0]);  
102.       return 0;  
103.     }  
104.     scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],                                                               point_cloud.sensor_origin_[1],                                             point_cloud.sensor_origin_[2])) *  
105.                         Eigen::Affine3f (point_cloud.sensor_orientation_);  
106.     std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";  
107.     if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)  
108.       std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";  
109.   }  
110.   else  
111.   {  
112.     setUnseenToMaxRange = true;  
113.     cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";  
114.     for (float x=-0.5f; x<=0.5f; x+=0.01f)  
115.     {  
116.       for (float y=-0.5f; y<=0.5f; y+=0.01f)  
117.       {  
118.         PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;  
119.         point_cloud.points.push_back (point);  
120.       }  
121.     }  
122.     point_cloud.width = (int) point_cloud.points.size ();  point_cloud.height = 1;  
123.   }  
124.  // -----从点云中创建深度图像----  
125. float noise_level = 0.0;  
126.   float min_range = 0.0f;  
127.   int border_size = 1;  
128.   boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);  
129.   pcl::RangeImage& range_image = *range_image_ptr;     
130.   range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),  
131.                                    scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);  
132.   range_image.integrateFarRanges (far_ranges);  
133.   if (setUnseenToMaxRange)  
134.     range_image.setUnseenToMaxRange ();  
135.     
136.   // -----打开三维观察仪并加入点云-----  
137.   pcl::visualization::PCLVisualizer viewer ("3D Viewer");  
138.   viewer.setBackgroundColor (1, 1, 1);  
139.   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);  
140.   viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");  
141.   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");  
142.   viewer.initCameraParameters ();  
143.  // setViewerPose (viewer, range_image.getTransformationToWorldSystem ());    
144.   // -----显示深度图像-----  
145.   pcl::visualization::RangeImageVisualizer  range_image_widget ("Range image");  
146.   range_image_widget.showRangeImage (range_image);  
147.   // -----提取 NARF 关键点-----  
148.   pcl::RangeImageBorderExtractor range_image_border_extractor;  
149.   pcl::NarfKeypoint narf_keypoint_detector;  
150.   narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);  
151.   narf_keypoint_detector.setRangeImage (&range_image);  
152.   narf_keypoint_detector.getParameters ().support_size = support_size;  
153.     
154.   pcl::PointCloud<int> keypoint_indices;  
155.   narf_keypoint_detector.compute (keypoint_indices);  
156.   std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";  
157.   // -----在深度图像窗口显示关键点-----  
158.   //for (size_t i=0; i<keypoint_indices.points.size (); ++i)  
159.     //range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,  
160.                                   //keypoint_indices.points[i]/range_image.width);  
161. // -----在三维视图中显示关键点 -----  
162.     pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);  
163.   pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;  
164.   keypoints.points.resize (keypoint_indices.points.size ());  
165.   for (size_t i=0; i<keypoint_indices.points.size (); ++i)  
166.     keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();  
167.   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);  
168.   viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");  
169.   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");  
170.   // -----为特征点提取NARF描述子-----  
171.   std::vector<int> keypoint_indices2;  
172.   keypoint_indices2.resize (keypoint_indices.points.size ());  
173.   for (unsigned int i=0; i<keypoint_indices.size (); ++i)   
174. //要得到正确的向量类型，这一步是必要的  
175.     keypoint_indices2[i]=keypoint_indices.points[i];  
176.   pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);  
177.   narf_descriptor.getParameters ().support_size = support_size;  
178.   narf_descriptor.getParameters ().rotation_invariant = rotation_invariant;  
179.   pcl::PointCloud<pcl::Narf36> narf_descriptors;  
180.   narf_descriptor.compute (narf_descriptors);  
181.   cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "  
182.                       <<keypoint_indices.points.size ()<< " keypoints.\n";  
183.   // -----Main loop-----  
184.   while (!viewer.wasStopped ())  
185.   {  
186. range_image_widget.spinOnce ();    
187. //处理用户交互事件  
188.     viewer.spinOnce ();  
189.     pcl_sleep(0.01);  
190.   }  
191. }  

运行：

2、NARF_Keypoint_Extraction

可以移植到Qt中，注意变量设置和参数定义，读取文件时保持与成员变量一致，后面的循环可以丢掉。

配置的时候可能会提示缺少下面的头文件，找到头文件，加上去即可：

[html]  view plain  copy

 

1. #include "vtkImageViewer.h"  
2. #include "vtkInteractionImageModule.h"  
3. #include "vtkInteractionWidgetsModule.h"  

narf_keypoint_extraction.cpp:

[html]  view plain  copy

 

1. #include <iostream>  
2.   
3. #include <boost/thread/thread.hpp>  
4. #include <pcl/range_image/range_image.h>  
5. #include <pcl/io/pcd_io.h>  
6. #include <pcl/visualization/range_image_visualizer.h>  
7. #include <pcl/visualization/pcl_visualizer.h>  
8. #include <pcl/features/range_image_border_extractor.h>  
9. #include <pcl/keypoints/narf_keypoint.h>  
10. //#include <pcl/console/parse.h>  
11.   
12. typedef pcl::PointXYZ PointT_XYZ;  
13.   
14. // --------------------  
15. // -----Parameters-----  
16. // --------------------  
17. float angular_resolution = 0.5f;  
18. float support_size = 0.2f;  
19. pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;  
20. bool setUnseenToMaxRange = false;  
21.   
22.   
23. // --------------  
24. // -----Main-----  
25. // --------------  
26. int  
27. main(int argc, char** argv)  
28. {  
29.   
30.         setUnseenToMaxRange = true;  
31.         cout << "Setting unseen values in range image to maximum range readings.\n";  
32.   
33.   
34.     angular_resolution = pcl::deg2rad(angular_resolution);  
35.   
36.     // ------------------------------------------------------------------  
37.     // -----Read pcd file or create example point cloud if not given-----  
38.     // ------------------------------------------------------------------  
39.     pcl::PointCloud<PointT_XYZ>::Ptr cloud(new pcl::PointCloud<PointT_XYZ>);  
40.     pcl::PointCloud<PointT_XYZ>& point_cloud = *cloud;  
41.     pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;  
42.     Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());  
43.   
44.     pcl::io::loadPCDFile("frame_00000.pcd", point_cloud);//frame_00000//bunny  
45.   
46.   
47.     // -----------------------------------------------  
48.     // -----Create RangeImage from the PointCloud-----  
49.     // -----------------------------------------------  
50.     float noise_level = 0.0;  
51.     float min_range = 0.0f;  
52.     int border_size = 1;  
53.     boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);  
54.     pcl::RangeImage& range_image = *range_image_ptr;  
55.     range_image.createFromPointCloud(point_cloud, angular_resolution, pcl::deg2rad(360.0f), pcl::deg2rad(180.0f),  
56.         scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);  
57.     range_image.integrateFarRanges(far_ranges);  
58.     if (setUnseenToMaxRange)  
59.         range_image.setUnseenToMaxRange();  
60.   
61.     // --------------------------------------------  
62.     // -----Open 3D viewer and add point cloud-----  
63.     // --------------------------------------------  
64.     pcl::visualization::PCLVisualizer viewer("3D Viewer");  
65.     viewer.setBackgroundColor(1, 0.5, 1);  
66.     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(range_image_ptr, 0, 0, 0);  
67.     viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");  
68.     viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");  
69.     //viewer.addCoordinateSystem (1.0f, "global");  
70.     //PointCloudColorHandlerCustom<PointT_XYZ> point_cloud_color_handler (cloud, 150, 150, 150);  
71.     //viewer.addPointCloud (cloud, point_cloud_color_handler, "original point cloud");  
72.     viewer.initCameraParameters();  
73.     //setViewerPose (viewer, range_image.getTransformationToWorldSystem ());  
74.   
75.     // --------------------------  
76.     // -----Show range image-----  
77.     // --------------------------  
78.     //pcl::visualization::RangeImageVisualizer range_image_widget("Range image");  
79.     //range_image_widget.showRangeImage(range_image);  
80.   
81.     // --------------------------------  
82.     // -----Extract NARF keypoints-----  
83.     // --------------------------------  
84.     pcl::RangeImageBorderExtractor range_image_border_extractor;  
85.     pcl::NarfKeypoint narf_keypoint_detector(&range_image_border_extractor);  
86.     narf_keypoint_detector.setRangeImage(&range_image);  
87.     narf_keypoint_detector.getParameters().support_size = support_size;  
88.     //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;  
89.     //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;  
90.   
91.     pcl::PointCloud<int> keypoint_indices;  
92.     narf_keypoint_detector.compute(keypoint_indices);  
93.     std::cout << "Found " << keypoint_indices.points.size() << " key points.\n";  
94.   
95.     // ----------------------------------------------  
96.     // -----Show keypoints in range image widget-----  
97.     // ----------------------------------------------  
98.     //for (size_t i=0; i<keypoint_indices.points.size (); ++i)  
99.     //range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,  
100.     //keypoint_indices.points[i]/range_image.width);  
101.   
102.     // -------------------------------------  
103.     // -----Show keypoints in 3D viewer-----  
104.     // -------------------------------------  
105.     pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);  
106.     pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;  
107.     keypoints.points.resize(keypoint_indices.points.size());  
108.     for (size_t i = 0; i < keypoint_indices.points.size(); ++i)  
109.         keypoints.points[i].getVector3fMap() = range_image.points[keypoint_indices.points[i]].getVector3fMap();  
110.   
111.     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler(keypoints_ptr, 0, 255, 0);  
112.     viewer.addPointCloud<pcl::PointXYZ>(keypoints_ptr, keypoints_color_handler, "keypoints");  
113.     viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");  
114.   
115.     //--------------------  
116.     // -----Main loop-----  
117.     //--------------------  
118.     while (!viewer.wasStopped())  
119.     {  
120. //      range_image_widget.spinOnce();  // process GUI events  
121.         viewer.spinOnce();  
122.         pcl_sleep(0.01);  
123.     }  
124. }  

转载：http://blog.csdn.net/sinat_24206709/article/details/51731992