narf_registration.cc
Go to the documentation of this file.
00001 /* \author Todor Stoyanov */
00002 
00003 #include <iostream>
00004 
00005 #include "pcl/range_image/range_image.h"
00006 #include "pcl/io/pcd_io.h"
00007 #include "pcl/features/range_image_border_extractor.h"
00008 #include "pcl/keypoints/narf_keypoint.h"
00009 #include <PointCloudUtils.hh>
00010 
00011 typedef pcl::PointXYZ PointType;
00012 
00013 // --------------------
00014 // -----Parameters-----
00015 // --------------------
00016 float angular_resolution = 0.5f;
00017 float support_size = 0.2f;
00018 pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
00019 bool setUnseenToMaxRange = false;
00020 
00021 // --------------
00022 // -----Help-----
00023 // --------------
00024 void
00025 printUsage (const char* progName)
00026 {
00027     std::cout << "\n\nUsage: "<<progName<<"moving_cloud.wrl static_cloud.wrl\n\n";
00028 }
00029 
00030 // --------------
00031 // -----Main-----
00032 // --------------
00033 int
00034 main (int argc, char** argv)
00035 {
00036     if(argc!=3)
00037     {
00038         printUsage(argv[0]);
00039         return 0;
00040     }
00041 
00042     pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
00043     pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
00044     pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
00045     Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
00046 
00047     std::string filename = argv[1];
00048     point_cloud = lslgeneric::readVRML(filename.c_str());
00049 
00050     scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
00051                                          point_cloud.sensor_origin_[1],
00052                                          point_cloud.sensor_origin_[2])) *
00053                         Eigen::Affine3f (point_cloud.sensor_orientation_);
00054 
00055     // -----------------------------------------------
00056     // -----Create RangeImage from the PointCloud-----
00057     // -----------------------------------------------
00058     float noise_level = 0.0;
00059     float min_range = 0.0f;
00060     int border_size = 1;
00061 
00062     boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
00063     pcl::RangeImage& range_image = *range_image_ptr;
00064     range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
00065                                       scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
00066     range_image.integrateFarRanges (far_ranges);
00067 
00068     // --------------------------------
00069     // -----Extract NARF keypoints-----
00070     // --------------------------------
00071     pcl::RangeImageBorderExtractor range_image_border_extractor;
00072     pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);
00073     narf_keypoint_detector.setRangeImage (&range_image);
00074     narf_keypoint_detector.getParameters ().support_size = support_size;
00075     //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
00076     //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
00077 
00078     pcl::PointCloud<int> keypoint_indices;
00079     narf_keypoint_detector.compute (keypoint_indices);
00080     std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";
00081 
00082     // -------------------------------------
00083     // -----Show keypoints in 3D viewer-----
00084     // -------------------------------------
00085     pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
00086     pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
00087     keypoints.points.resize (keypoint_indices.points.size ());
00088     for (size_t i=0; i<keypoint_indices.points.size (); ++i)
00089         keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();
00090 
00091     //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
00092     //viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
00093     //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
00094 
00095 }
00096 


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Wed Aug 26 2015 15:24:52