multiscale_feature_persistence_example.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *      
00036  */
00037 
00038 #include <pcl/features/multiscale_feature_persistence.h>
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/filters/voxel_grid.h>
00041 #include <pcl/filters/extract_indices.h>
00042 #include <pcl/features/normal_3d.h>
00043 #include <pcl/features/fpfh.h>
00044 
00045 #include <pcl/visualization/cloud_viewer.h>
00046 
00047 using namespace pcl;
00048 
00049 const Eigen::Vector4f subsampling_leaf_size (0.01f, 0.01f, 0.01f, 0.0f);
00050 const float normal_estimation_search_radius = 0.05f;
00051 
00052 
00053 void
00054 subsampleAndCalculateNormals (PointCloud<PointXYZ>::Ptr &cloud,
00055                               PointCloud<PointXYZ>::Ptr &cloud_subsampled,
00056                               PointCloud<Normal>::Ptr &cloud_subsampled_normals)
00057 {
00058   cloud_subsampled = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ> ());
00059   VoxelGrid<PointXYZ> subsampling_filter;
00060   subsampling_filter.setInputCloud (cloud);
00061   subsampling_filter.setLeafSize (subsampling_leaf_size);
00062   subsampling_filter.filter (*cloud_subsampled);
00063 
00064   cloud_subsampled_normals = PointCloud<Normal>::Ptr (new PointCloud<Normal> ());
00065   NormalEstimation<PointXYZ, Normal> normal_estimation_filter;
00066   normal_estimation_filter.setInputCloud (cloud_subsampled);
00067   pcl::search::KdTree<PointXYZ>::Ptr search_tree (new pcl::search::KdTree<PointXYZ>);
00068   normal_estimation_filter.setSearchMethod (search_tree);
00069   normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
00070   normal_estimation_filter.compute (*cloud_subsampled_normals);
00071 }
00072 
00073 
00074 int
00075 main (int argc, char **argv)
00076 {
00077   if (argc != 2)
00078   {
00079     PCL_ERROR ("Syntax: ./multiscale_feature_persistence_example [path_to_cloud.pcl]\n");
00080     return -1;
00081   }
00082 
00083   PointCloud<PointXYZ>::Ptr cloud_scene (new PointCloud<PointXYZ> ());
00084   PCDReader reader;
00085   reader.read (argv[1], *cloud_scene);
00086 
00087   PointCloud<PointXYZ>::Ptr cloud_subsampled;
00088   PointCloud<Normal>::Ptr cloud_subsampled_normals;
00089   subsampleAndCalculateNormals (cloud_scene, cloud_subsampled, cloud_subsampled_normals);
00090 
00091   PCL_INFO ("STATS:\ninitial point cloud size: %u\nsubsampled point cloud size: %u\n", cloud_scene->points.size (), cloud_subsampled->points.size ());
00092   visualization::CloudViewer viewer ("Multiscale Feature Persistence Example Visualization");
00093   viewer.showCloud (cloud_scene, "scene");
00094 
00095 
00096   MultiscaleFeaturePersistence<PointXYZ, FPFHSignature33> feature_persistence;
00097   std::vector<float> scale_values;
00098   for (float x = 2.0f; x < 3.6f; x += 0.35f)
00099     scale_values.push_back (x / 100.0f);
00100   feature_persistence.setScalesVector (scale_values);
00101   feature_persistence.setAlpha (1.3f);
00102   FPFHEstimation<PointXYZ, Normal, FPFHSignature33>::Ptr fpfh_estimation (new FPFHEstimation<PointXYZ, Normal, FPFHSignature33> ());
00103   fpfh_estimation->setInputCloud (cloud_subsampled);
00104   fpfh_estimation->setInputNormals (cloud_subsampled_normals);
00105   pcl::search::KdTree<PointXYZ>::Ptr tree (new pcl::search::KdTree<PointXYZ> ());
00106   fpfh_estimation->setSearchMethod (tree);
00107   feature_persistence.setFeatureEstimator (fpfh_estimation);
00108   feature_persistence.setDistanceMetric (pcl::CS);
00109 
00110   PointCloud<FPFHSignature33>::Ptr output_features (new PointCloud<FPFHSignature33> ());
00111   boost::shared_ptr<std::vector<int> > output_indices (new std::vector<int> ());
00112   feature_persistence.determinePersistentFeatures (*output_features, output_indices);
00113 
00114   PCL_INFO ("persistent features cloud size: %u\n", output_features->points.size ());
00115 
00116   ExtractIndices<PointXYZ> extract_indices_filter;
00117   extract_indices_filter.setInputCloud (cloud_subsampled);
00118   extract_indices_filter.setIndices (output_indices);
00119   PointCloud<PointXYZ>::Ptr persistent_features_locations (new PointCloud<PointXYZ> ());
00120   extract_indices_filter.filter (*persistent_features_locations);
00121 
00122   viewer.showCloud (persistent_features_locations, "persistent features");
00123   PCL_INFO ("Persistent features have been computed. Waiting for the user to quit the visualization window.\n");
00124 
00125 //  io::savePCDFileASCII ("persistent_features.pcd", *persistent_features_locations);
00126 //  PCL_INFO ("\nPersistent feature locations written to persistent_features.pcd\n");
00127 
00128   while (!viewer.wasStopped (50)) {}
00129 
00130   return (0);
00131 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:42