smoothed_surfaces_keypoint.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Alexandru-Eugen Ichim
00005  *                      Willow Garage, Inc
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * $Id$
00036  */
00037 
00038 #ifndef PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
00039 #define PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
00040 
00041 #include <pcl/keypoints/smoothed_surfaces_keypoint.h>
00042 #include <pcl/kdtree/kdtree_flann.h>
00043 
00044 //#include <pcl/io/pcd_io.h>
00045 
00047 template <typename PointT, typename PointNT> void
00048 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::addSmoothedPointCloud (const PointCloudTConstPtr &cloud,
00049                                                                        const PointCloudNTConstPtr &normals,
00050                                                                        KdTreePtr &kdtree,
00051                                                                        float &scale)
00052 {
00053   clouds_.push_back (cloud);
00054   cloud_normals_.push_back (normals);
00055   cloud_trees_.push_back (kdtree);
00056   scales_.push_back (std::pair<float, size_t> (scale, scales_.size ()));
00057 }
00058 
00059 
00061 template <typename PointT, typename PointNT> void
00062 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::resetClouds ()
00063 {
00064   clouds_.clear ();
00065   cloud_normals_.clear ();
00066   scales_.clear ();
00067 }
00068 
00069 
00071 template <typename PointT, typename PointNT> void
00072 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &output)
00073 {
00074   // Calculate differences for each cloud
00075   std::vector<std::vector<float> > diffs (scales_.size ());
00076 
00077   // The cloud with the smallest scale has no differences
00078   std::vector<float> aux_diffs (input_->points.size (), 0.0f);
00079   diffs[scales_[0].second] = aux_diffs;
00080 
00081   cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]);
00082   for (size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i)
00083   {
00084     size_t cloud_i = scales_[scale_i].second,
00085         cloud_i_minus_one = scales_[scale_i - 1].second;
00086     diffs[cloud_i].resize (input_->points.size ());
00087     PCL_INFO ("cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one);
00088     for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
00089       diffs[cloud_i][point_i] = cloud_normals_[cloud_i]->points[point_i].getNormalVector3fMap ().dot (
00090           clouds_[cloud_i]->points[point_i].getVector3fMap () - clouds_[cloud_i_minus_one]->points[point_i].getVector3fMap ());
00091 
00092     // Setup kdtree for this cloud
00093     cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]);
00094   }
00095 
00096 
00097   // Find minima and maxima in differences inside the input cloud
00098   typename pcl::search::Search<PointT>::Ptr input_tree = cloud_trees_.back ();
00099   for (int point_i = 0; point_i < static_cast<int> (input_->points.size ()); ++point_i)
00100   {
00101     std::vector<int> nn_indices;
00102     std::vector<float> nn_distances;
00103     input_tree->radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances);
00104 
00105     bool is_min = true, is_max = true;
00106     for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
00107       if (*nn_it != point_i)
00108       {
00109         if (diffs[input_index_][point_i] < diffs[input_index_][*nn_it])
00110           is_max = false;
00111         else if (diffs[input_index_][point_i] > diffs[input_index_][*nn_it])
00112           is_min = false;
00113       }
00114 
00115     // If the point is a local minimum/maximum, check if it is the same over all the scales
00116     if (is_min || is_max)
00117     {
00118       bool passed_min = true, passed_max = true;
00119       for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
00120       {
00121         size_t cloud_i = scales_[scale_i].second;
00122         // skip input cloud
00123         if (cloud_i == clouds_.size () - 1)
00124           continue;
00125 
00126         nn_indices.clear (); nn_distances.clear ();
00127         cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances);
00128 
00129         bool is_min_other_scale = true, is_max_other_scale = true;
00130         for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
00131           if (*nn_it != point_i)
00132           {
00133             if (diffs[input_index_][point_i] < diffs[cloud_i][*nn_it])
00134               is_max_other_scale = false;
00135             else if (diffs[input_index_][point_i] > diffs[cloud_i][*nn_it])
00136               is_min_other_scale = false;
00137           }
00138 
00139         if (is_min == true && is_min_other_scale == false)
00140           passed_min = false;
00141         if (is_max == true && is_max_other_scale == false)
00142           passed_max = false;
00143 
00144         if (!passed_min && !passed_max)
00145           break;
00146       }
00147 
00148       // check if point was minimum/maximum over all the scales
00149       if (passed_min || passed_max)
00150         output.points.push_back (input_->points[point_i]);
00151     }
00152   }
00153 
00154   output.header = input_->header;
00155   output.width = static_cast<uint32_t> (output.points.size ());
00156   output.height = 1;
00157 
00158   // debug stuff
00159 //  for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
00160 //  {
00161 //    PointCloud<PointXYZI>::Ptr debug_cloud (new PointCloud<PointXYZI> ());
00162 //    debug_cloud->points.resize (input_->points.size ());
00163 //    debug_cloud->width = input_->width;
00164 //    debug_cloud->height = input_->height;
00165 //    for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
00166 //    {
00167 //      debug_cloud->points[point_i].intensity = diffs[scales_[scale_i].second][point_i];
00168 //      debug_cloud->points[point_i].x = input_->points[point_i].x;
00169 //      debug_cloud->points[point_i].y = input_->points[point_i].y;
00170 //      debug_cloud->points[point_i].z = input_->points[point_i].z;
00171 //    }
00172 
00173 //    char str[512]; sprintf (str, "diffs_%2d.pcd", scale_i);
00174 //    io::savePCDFile (str, *debug_cloud);
00175 //  }
00176 }
00177 
00179 template <typename PointT, typename PointNT> bool
00180 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::initCompute ()
00181 {
00182   PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n");
00183   if ( !Keypoint<PointT, PointT>::initCompute ())
00184   {
00185     PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
00186     return false;
00187   }
00188 
00189   if (!normals_)
00190   {
00191     PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
00192     return false;
00193   }
00194   if (clouds_.size () == 0)
00195   {
00196     PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
00197     return false;
00198   }
00199 
00200   if (input_->points.size () != normals_->points.size ())
00201   {
00202     PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
00203     return false;
00204   }
00205 
00206   for (size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
00207   {
00208     if (clouds_[cloud_i]->points.size () != input_->points.size ())
00209     {
00210       PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i);
00211       return false;
00212     }
00213 
00214     if (cloud_normals_[cloud_i]->points.size () != input_->points.size ())
00215     {
00216       PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i);
00217       return false;
00218     }
00219   }
00220 
00221   // Add the input cloud as the last index
00222   scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ()));
00223   clouds_.push_back (input_);
00224   cloud_normals_.push_back (normals_);
00225   cloud_trees_.push_back (tree_);
00226   // Sort the clouds by their scales
00227   sort (scales_.begin (), scales_.end (), compareScalesFunction);
00228 
00229   // Find the index of the input after sorting
00230   for (size_t i = 0; i < scales_.size (); ++i)
00231     if (scales_[i].second == scales_.size () - 1)
00232     {
00233       input_index_ = i;
00234       break;
00235     }
00236 
00237   PCL_INFO ("Scales: ");
00238   for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
00239   PCL_INFO ("\n");
00240 
00241   return true;
00242 }
00243 
00244 
00245 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>;
00246 
00247 
00248 #endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:33:37