multiscale_feature_persistence_example.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2009-2012, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *      
 */

#include <pcl/features/multiscale_feature_persistence.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>

#include <pcl/visualization/cloud_viewer.h>

using namespace pcl;

const Eigen::Vector4f subsampling_leaf_size (0.01f, 0.01f, 0.01f, 0.0f);
const float normal_estimation_search_radius = 0.05f;


void
subsampleAndCalculateNormals (PointCloud<PointXYZ>::Ptr &cloud,
                              PointCloud<PointXYZ>::Ptr &cloud_subsampled,
                              PointCloud<Normal>::Ptr &cloud_subsampled_normals)
{
  cloud_subsampled = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ> ());
  VoxelGrid<PointXYZ> subsampling_filter;
  subsampling_filter.setInputCloud (cloud);
  subsampling_filter.setLeafSize (subsampling_leaf_size);
  subsampling_filter.filter (*cloud_subsampled);

  cloud_subsampled_normals = PointCloud<Normal>::Ptr (new PointCloud<Normal> ());
  NormalEstimation<PointXYZ, Normal> normal_estimation_filter;
  normal_estimation_filter.setInputCloud (cloud_subsampled);
  pcl::search::KdTree<PointXYZ>::Ptr search_tree (new pcl::search::KdTree<PointXYZ>);
  normal_estimation_filter.setSearchMethod (search_tree);
  normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
  normal_estimation_filter.compute (*cloud_subsampled_normals);
}


int
main (int argc, char **argv)
{
  if (argc != 2)
  {
    PCL_ERROR ("Syntax: ./multiscale_feature_persistence_example [path_to_cloud.pcl]\n");
    return -1;
  }

  PointCloud<PointXYZ>::Ptr cloud_scene (new PointCloud<PointXYZ> ());
  PCDReader reader;
  reader.read (argv[1], *cloud_scene);

  PointCloud<PointXYZ>::Ptr cloud_subsampled;
  PointCloud<Normal>::Ptr cloud_subsampled_normals;
  subsampleAndCalculateNormals (cloud_scene, cloud_subsampled, cloud_subsampled_normals);

  PCL_INFO ("STATS:\ninitial point cloud size: %u\nsubsampled point cloud size: %u\n", cloud_scene->points.size (), cloud_subsampled->points.size ());
  visualization::CloudViewer viewer ("Multiscale Feature Persistence Example Visualization");
  viewer.showCloud (cloud_scene, "scene");


  MultiscaleFeaturePersistence<PointXYZ, FPFHSignature33> feature_persistence;
  std::vector<float> scale_values;
  for (float x = 2.0f; x < 3.6f; x += 0.35f)
    scale_values.push_back (x / 100.0f);
  feature_persistence.setScalesVector (scale_values);
  feature_persistence.setAlpha (1.3f);
  FPFHEstimation<PointXYZ, Normal, FPFHSignature33>::Ptr fpfh_estimation (new FPFHEstimation<PointXYZ, Normal, FPFHSignature33> ());
  fpfh_estimation->setInputCloud (cloud_subsampled);
  fpfh_estimation->setInputNormals (cloud_subsampled_normals);
  pcl::search::KdTree<PointXYZ>::Ptr tree (new pcl::search::KdTree<PointXYZ> ());
  fpfh_estimation->setSearchMethod (tree);
  feature_persistence.setFeatureEstimator (fpfh_estimation);
  feature_persistence.setDistanceMetric (pcl::CS);

  PointCloud<FPFHSignature33>::Ptr output_features (new PointCloud<FPFHSignature33> ());
  boost::shared_ptr<std::vector<int> > output_indices (new std::vector<int> ());
  feature_persistence.determinePersistentFeatures (*output_features, output_indices);

  PCL_INFO ("persistent features cloud size: %u\n", output_features->points.size ());

  ExtractIndices<PointXYZ> extract_indices_filter;
  extract_indices_filter.setInputCloud (cloud_subsampled);
  extract_indices_filter.setIndices (output_indices);
  PointCloud<PointXYZ>::Ptr persistent_features_locations (new PointCloud<PointXYZ> ());
  extract_indices_filter.filter (*persistent_features_locations);

  viewer.showCloud (persistent_features_locations, "persistent features");
  PCL_INFO ("Persistent features have been computed. Waiting for the user to quit the visualization window.\n");

//  io::savePCDFileASCII ("persistent_features.pcd", *persistent_features_locations);
//  PCL_INFO ("\nPersistent feature locations written to persistent_features.pcd\n");

  while (!viewer.wasStopped (50));

  return 0;
}