narf_extraction.cc
Go to the documentation of this file.
00001 /* \author Bastian Steder */
00002 
00003 #include <iostream>
00004 
00005 #include <boost/thread/thread.hpp>
00006 #include "pcl/range_image/range_image.h"
00007 #include "pcl/io/pcd_io.h"
00008 //#include "pcl/visualization/range_image_visualizer.h"
00009 #include "pcl/visualization/pcl_visualizer.h"
00010 #include "pcl/features/range_image_border_extractor.h"
00011 #include "pcl/keypoints/narf_keypoint.h"
00012 #include <pcl/console/parse.h>
00013 
00014 #include <PointCloudUtils.hh>
00015 
00016 typedef pcl::PointXYZ PointType;
00017 
00018 // --------------------
00019 // -----Parameters-----
00020 // --------------------
00021 float angular_resolution = 0.5f;
00022 float support_size = 0.2f;
00023 pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
00024 bool setUnseenToMaxRange = false;
00025 
00026 // --------------
00027 // -----Help-----
00028 // --------------
00029 void
00030 printUsage (const char* progName)
00031 {
00032     std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
00033               << "Options:\n"
00034               << "-------------------------------------------\n"
00035               << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
00036               << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
00037               << "-m           Treat all unseen points as maximum range readings\n"
00038               << "-s <float>   support size for the interest points (diameter of the used sphere - "
00039               <<                                                     "default "<<support_size<<")\n"
00040               << "-h           this help\n"
00041               << "\n\n";
00042 }
00043 
00044 void
00045 setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
00046 {
00047     Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
00048     Eigen::Vector3f look_at_vector = pcl::getRotationOnly (viewer_pose) * Eigen::Vector3f (0, 0, 1) + pos_vector;
00049     Eigen::Vector3f up_vector = pcl::getRotationOnly (viewer_pose) * Eigen::Vector3f (0, -1, 0);
00050     viewer.camera_.pos[0] = pos_vector[0];
00051     viewer.camera_.pos[1] = pos_vector[1];
00052     viewer.camera_.pos[2] = pos_vector[2];
00053     viewer.camera_.focal[0] = look_at_vector[0];
00054     viewer.camera_.focal[1] = look_at_vector[1];
00055     viewer.camera_.focal[2] = look_at_vector[2];
00056     viewer.camera_.view[0] = up_vector[0];
00057     viewer.camera_.view[1] = up_vector[1];
00058     viewer.camera_.view[2] = up_vector[2];
00059     viewer.updateCamera ();
00060 }
00061 
00062 // --------------
00063 // -----Main-----
00064 // --------------
00065 int
00066 main (int argc, char** argv)
00067 {
00068     // --------------------------------------
00069     // -----Parse Command Line Arguments-----
00070     // --------------------------------------
00071     if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00072     {
00073         printUsage (argv[0]);
00074         return 0;
00075     }
00076     if (pcl::console::find_argument (argc, argv, "-m") >= 0)
00077     {
00078         setUnseenToMaxRange = true;
00079         cout << "Setting unseen values in range image to maximum range readings.\n";
00080     }
00081     int tmp_coordinate_frame;
00082     if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
00083     {
00084         coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
00085         cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
00086     }
00087     if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
00088         cout << "Setting support size to "<<support_size<<".\n";
00089     if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
00090         cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
00091     angular_resolution = pcl::deg2rad (angular_resolution);
00092 
00093     // ------------------------------------------------------------------
00094     // -----Read pcd file or create example point cloud if not given-----
00095     // ------------------------------------------------------------------
00096     pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
00097     pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
00098     pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
00099     Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
00100     std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "wrl");
00101     if (!pcd_filename_indices.empty ())
00102     {
00103         std::string filename = argv[pcd_filename_indices[0]];
00104         point_cloud = lslgeneric::readVRML(filename.c_str());
00105         /*
00106         if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
00107         {
00108             cerr << "Was not able to open file \""<<filename<<"\".\n";
00109             printUsage (argv[0]);
00110             return 0;
00111         }
00112         */
00113         scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
00114                                              point_cloud.sensor_origin_[1],
00115                                              point_cloud.sensor_origin_[2])) *
00116                             Eigen::Affine3f (point_cloud.sensor_orientation_);
00117         std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
00118         if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
00119             std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
00120     }
00121     else
00122     {
00123         cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
00124         for (float x=-0.5f; x<=0.5f; x+=0.01f)
00125         {
00126             for (float y=-0.5f; y<=0.5f; y+=0.01f)
00127             {
00128                 PointType point;
00129                 point.x = x;
00130                 point.y = y;
00131                 point.z = 2.0f - y;
00132                 point_cloud.points.push_back (point);
00133             }
00134         }
00135         point_cloud.width = point_cloud.points.size ();
00136         point_cloud.height = 1;
00137     }
00138 
00139     // -----------------------------------------------
00140     // -----Create RangeImage from the PointCloud-----
00141     // -----------------------------------------------
00142     float noise_level = 0.0;
00143     float min_range = 0.0f;
00144     int border_size = 1;
00145     boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
00146     pcl::RangeImage& range_image = *range_image_ptr;
00147     range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
00148                                       scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
00149     range_image.integrateFarRanges (far_ranges);
00150     if (setUnseenToMaxRange)
00151         range_image.setUnseenToMaxRange ();
00152 
00153     // --------------------------------------------
00154     // -----Open 3D viewer and add point cloud-----
00155     // --------------------------------------------
00156     pcl::visualization::PCLVisualizer viewer ("3D Viewer");
00157     viewer.setBackgroundColor (1, 1, 1);
00158     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
00159     viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
00160     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
00161     //viewer.addCoordinateSystem (1.0f);
00162     //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
00163     //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
00164     viewer.initCameraParameters ();
00165     setViewerPose (viewer, range_image.getTransformationToWorldSystem ());
00166 
00167     // --------------------------
00168     // -----Show range image-----
00169     // --------------------------
00170 //    pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
00171 //    range_image_widget.setRangeImage (range_image);
00172 
00173     // --------------------------------
00174     // -----Extract NARF keypoints-----
00175     // --------------------------------
00176     pcl::RangeImageBorderExtractor range_image_border_extractor;
00177     pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);
00178     narf_keypoint_detector.setRangeImage (&range_image);
00179     narf_keypoint_detector.getParameters ().support_size = support_size;
00180     //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
00181     //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
00182 
00183     pcl::PointCloud<int> keypoint_indices;
00184     narf_keypoint_detector.compute (keypoint_indices);
00185     std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";
00186 
00187     // ----------------------------------------------
00188     // -----Show keypoints in range image widget-----
00189     // ----------------------------------------------
00190 //    for (size_t i=0; i<keypoint_indices.points.size (); ++i)
00191 //      range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
00192 //              keypoint_indices.points[i]/range_image.width);
00193 
00194     // -------------------------------------
00195     // -----Show keypoints in 3D viewer-----
00196     // -------------------------------------
00197     pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
00198     pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
00199     keypoints.points.resize (keypoint_indices.points.size ());
00200     for (size_t i=0; i<keypoint_indices.points.size (); ++i)
00201         keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();
00202 
00203     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
00204     viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
00205     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
00206 
00207     //--------------------
00208     // -----Main loop-----
00209     //--------------------
00210     while (!viewer.wasStopped ()) // && range_image_widget.isShown ())
00211     {
00212         //pcl::visualization::ImageWidgetWX::spinOnce ();  // process GUI events
00213         viewer.spinOnce (100);
00214         boost::this_thread::sleep (boost::posix_time::microseconds (100000));
00215     }
00216 }
00217 


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:30