keypoints indices feature

examples/keypoints/CMakeLists.txt

@@ -19,4 +19,7 @@ if(BUILD_visualization)
   PCL_ADD_EXAMPLE(pcl_example_sift_z_keypoint_estimation FILES example_sift_z_keypoint_estimation.cpp
 		                LINK_WITH pcl_common  pcl_visualization pcl_keypoints pcl_io)
 
+  PCL_ADD_EXAMPLE(pcl_example_get_keypoints_indices FILES example_get_keypoints_indices.cpp
+                  LINK_WITH pcl_common pcl_keypoints pcl_io)
+
 endif(BUILD_visualization)

examples/keypoints/example_get_keypoints_indices.cpp

@@ -0,0 +1,82 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2013-, Open Perception, 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 <iostream>
+
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/keypoints/harris_3d.h>
+
+int
+main(int argc, char** argv)
+{
+  if (argc < 2)
+  {
+    pcl::console::print_info ("Keypoints indices example application.\n");
+    pcl::console::print_info ("Syntax is: %s <source-pcd-file>\n", argv[0]);
+    return (1);
+  }
+
+  pcl::console::print_info ("Reading %s\n", argv[1]);
+
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+  if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (argv[1], *cloud) == -1) // load the file
+  {
+    pcl::console::print_error ("Couldn't read file %s!\n", argv[1]);
+    return (-1);
+  }
+
+  pcl::HarrisKeypoint3D <pcl::PointXYZRGB, pcl::PointXYZI> detector;
+  pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints (new pcl::PointCloud<pcl::PointXYZI>);
+  detector.setNonMaxSupression (true);
+  detector.setInputCloud (cloud);
+  detector.setThreshold (1e-6);
+  pcl::StopWatch watch;
+  detector.compute (*keypoints);
+  pcl::console::print_highlight ("Detected %zd points in %dms\n", keypoints->size (), watch.getTimeSeconds ());
+  pcl::PointIndicesConstPtr keypoints_indices = detector.getKeypointsIndices ();
+  if (!keypoints_indices->indices.empty ())
+  {
+    pcl::io::savePCDFile ("keypoints.pcd", *cloud, keypoints_indices->indices, true);
+    pcl::console::print_info ("Saved keypoints to keypoints.pcd\n");
+  }
+  else
+    pcl::console::print_warn ("Keypoints indices are empty!\n");
+}

keypoints/include/pcl/keypoints/harris_2d.h

@@ -65,6 +65,7 @@ namespace pcl
       using Keypoint<PointInT, PointOutT>::name_;
       using Keypoint<PointInT, PointOutT>::input_;
       using Keypoint<PointInT, PointOutT>::indices_;
+      using Keypoint<PointInT, PointOutT>::keypoints_indices_;
 
       typedef enum {HARRIS = 1, NOBLE, LOWE, TOMASI} ResponseMethod;
 

keypoints/include/pcl/keypoints/harris_3d.h

@@ -72,6 +72,7 @@ namespace pcl
       using Keypoint<PointInT, PointOutT>::k_;
       using Keypoint<PointInT, PointOutT>::search_radius_;
       using Keypoint<PointInT, PointOutT>::search_parameter_;
+      using Keypoint<PointInT, PointOutT>::keypoints_indices_;
       using Keypoint<PointInT, PointOutT>::initCompute;
 
       typedef enum {HARRIS = 1, NOBLE, LOWE, TOMASI, CURVATURE} ResponseMethod;

keypoints/include/pcl/keypoints/harris_6d.h

@@ -66,6 +66,7 @@ namespace pcl
       using Keypoint<PointInT, PointOutT>::k_;
       using Keypoint<PointInT, PointOutT>::search_radius_;
       using Keypoint<PointInT, PointOutT>::search_parameter_;
+      using Keypoint<PointInT, PointOutT>::keypoints_indices_;
 
       /**
        * @brief Constructor

keypoints/include/pcl/keypoints/impl/harris_2d.hpp

@@ -250,7 +250,11 @@ pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCl
   }
 
   if (!nonmax_)
+  {
     output = *response_;
+    for (size_t i = 0; i < response->size (); ++i)
+      keypoints_indices_->indices.push_back (i);
+  }
   else
   {    
     threshold_*= highest_response_;
@@ -276,7 +280,10 @@ pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCl
 #ifdef _OPENMP
 #pragma omp critical
 #endif
-      output.push_back (response_->at (indices_->at (idx)));
+      {
+        output.push_back (response_->at (indices_->at (idx)));
+        keypoints_indices_->indices.push_back (indices_->at (idx));
+      }
 
 			int u_end = std::min (width, indices_->at (idx) % width + min_distance_);
 			int v_end = std::min (height, indices_->at (idx) / width + min_distance_);

keypoints/include/pcl/keypoints/impl/harris_3d.hpp

@@ -223,6 +223,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::initCompute ()
     PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_);
     return (false);
   }
+
   return (true);
 }
 
@@ -258,6 +259,8 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
     output = *response;
     // we do not change the denseness in this case
     output.is_dense = input_->is_dense;
+    for (size_t i = 0; i < response->size (); ++i)
+      keypoints_indices_->indices.push_back (i);
   }
   else
   {
@@ -290,7 +293,10 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
 #ifdef _OPENMP
 #pragma omp critical
 #endif
+      {
         output.points.push_back (response->points[idx]);
+        keypoints_indices_->indices.push_back (idx);
+      }
     }
 
     if (refine_)

keypoints/include/pcl/keypoints/impl/harris_6d.hpp

@@ -219,6 +219,8 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
     output = *response;
     // we do not change the denseness in this case
     output.is_dense = input_->is_dense;
+    for (size_t i = 0; i < response->size (); ++i)
+      keypoints_indices_->indices.push_back (i);
   }
   else
   {
@@ -249,7 +251,10 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
 #ifdef _OPENMP
         #pragma omp critical
 #endif
+      {
         output.points.push_back (response->points[idx]);
+        keypoints_indices_->indices.push_back (idx);
+      }
     }
 
     if (refine_)
@@ -259,8 +264,6 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
     output.width = static_cast<uint32_t> (output.points.size());
     output.is_dense = true;
   }
-
-
 }
 
 template <typename PointInT, typename PointOutT, typename NormalT> void

keypoints/include/pcl/keypoints/impl/iss_3d.hpp

@@ -437,6 +437,7 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut
       PointOutT p;
       p.getVector3fMap () = input_->points[index].getVector3fMap ();
       output.points.push_back(p);
+      keypoints_indices_->indices.push_back (index);
     }
   }
 

keypoints/include/pcl/keypoints/impl/keypoint.hpp

@@ -113,6 +113,9 @@ pcl::Keypoint<PointInT, PointOutT>::initCompute ()
     }
   }
 
+  keypoints_indices_.reset (new pcl::PointIndices);
+  keypoints_indices_->indices.reserve (input_->size ());
+
   return (true);
 }
 

keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp

@@ -147,7 +147,10 @@ pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &ou
 
       // check if point was minimum/maximum over all the scales
       if (passed_min || passed_max)
+      {
         output.points.push_back (input_->points[point_i]);
+        keypoints_indices_->indices.push_back (point_i);
+      }
     }
   }
 
@@ -238,7 +241,7 @@ pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::initCompute ()
   for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
   PCL_INFO ("\n");
 
-  return true;
+  return (true);
 }
 
 

keypoints/include/pcl/keypoints/impl/susan.hpp

@@ -246,6 +246,7 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::initCompute ()
     PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
     return (false);
   }
+
   return (true);
 }
 
@@ -413,7 +414,13 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (P
   response->width = static_cast<uint32_t> (response->size ());
 
   if (!nonmax_)
+  {
     output = *response;
+    for (size_t i = 0; i < response->size (); ++i)
+      keypoints_indices_->indices.push_back (i);
+    // we don not change the denseness
+    output.is_dense = input_->is_dense;
+  }
   else
   {
     output.points.clear ();
@@ -447,14 +454,16 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (P
 //#ifdef _OPENMP
 //#pragma omp critical
 //#endif
+      {
         output.points.push_back (response->points[idx]);
+        keypoints_indices_->indices.push_back (idx);
+      }
     }
 
     output.height = 1;
-    output.width = static_cast<uint32_t> (output.points.size());  
+    output.width = static_cast<uint32_t> (output.points.size());
+    output.is_dense = true;
   }
-  // we don not change the denseness
-  output.is_dense = input_->is_dense;
 }
 
 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;

keypoints/include/pcl/keypoints/iss_3d.h

@@ -104,6 +104,7 @@ namespace pcl
       using Keypoint<PointInT, PointOutT>::tree_;
       using Keypoint<PointInT, PointOutT>::search_radius_;
       using Keypoint<PointInT, PointOutT>::search_parameter_;
+      using Keypoint<PointInT, PointOutT>::keypoints_indices_;
 
       /** \brief Constructor.
         * \param[in] salient_radius the radius of the spherical neighborhood used to compute the scatter matrix.

keypoints/include/pcl/keypoints/keypoint.h

@@ -133,6 +133,12 @@ namespace pcl
       inline double
       getRadiusSearch () { return (search_radius_); }
 
+      /** \brief \return the keypoints indices in the input cloud.
+        * \note not all the daughter classes populate the keypoints indices so check emptiness before use.
+        */
+      pcl::PointIndicesConstPtr
+      getKeypointsIndices () { return (keypoints_indices_); }
+
       /** \brief Base method for key point detection for all points given in <setInputCloud (), setIndices ()> using
         * the surface in setSearchSurface () and the spatial locator in setSearchMethod ()
         * \param output the resultant point cloud model dataset containing the estimated features
@@ -187,6 +193,9 @@ namespace pcl
       /** \brief The number of K nearest neighbors to use for each point. */
       int k_;
 
+      /** \brief Indices of the keypoints in the input cloud. */
+      pcl::PointIndicesPtr keypoints_indices_;
+
       /** \brief Get a string representation of the name of this class. */
       inline const std::string&
       getClassName () const { return (name_); }

keypoints/include/pcl/keypoints/smoothed_surfaces_keypoint.h

@@ -61,6 +61,7 @@ namespace pcl
       using PCLBase<PointT>::input_;
       using Keypoint<PointT, PointT>::name_;
       using Keypoint<PointT, PointT>::tree_;
+      using Keypoint<PointT, PointT>::keypoints_indices_;
       using Keypoint<PointT, PointT>::initCompute;
 
       typedef pcl::PointCloud<PointT> PointCloudT;

keypoints/include/pcl/keypoints/susan.h

@@ -77,6 +77,7 @@ namespace pcl
       using Keypoint<PointInT, PointOutT>::k_;
       using Keypoint<PointInT, PointOutT>::search_radius_;
       using Keypoint<PointInT, PointOutT>::search_parameter_;
+      using Keypoint<PointInT, PointOutT>::keypoints_indices_;
       using Keypoint<PointInT, PointOutT>::initCompute;
 
       /** \brief Constructor