uniform_sampling.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id$
00035  *
00036  */
00037 
00038 #ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
00039 #define PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
00040 
00041 #include <pcl/common/common.h>
00042 #include <pcl/keypoints/uniform_sampling.h>
00043 
00045 template <typename PointInT> void
00046 pcl::UniformSampling<PointInT>::detectKeypoints (PointCloudOut &output)
00047 {
00048   // Has the input dataset been set already?
00049   if (!input_)
00050   {
00051     PCL_WARN ("[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
00052     output.width = output.height = 0;
00053     output.points.clear ();
00054     return;
00055   }
00056 
00057   output.height       = 1;                    // downsampling breaks the organized structure
00058   output.is_dense     = true;                 // we filter out invalid points
00059 
00060   Eigen::Vector4f min_p, max_p;
00061   // Get the minimum and maximum dimensions
00062   pcl::getMinMax3D<PointInT>(*input_, min_p, max_p);
00063 
00064   // Compute the minimum and maximum bounding box values
00065   min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
00066   max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
00067   min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
00068   max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
00069   min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
00070   max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
00071 
00072   // Compute the number of divisions needed along all axis
00073   div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
00074   div_b_[3] = 0;
00075 
00076   // Clear the leaves
00077   leaves_.clear ();
00078 
00079   // Set up the division multiplier
00080   divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
00081 
00082   // First pass: build a set of leaves with the point index closest to the leaf center
00083   for (size_t cp = 0; cp < indices_->size (); ++cp)
00084   {
00085     if (!input_->is_dense)
00086       // Check if the point is invalid
00087       if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) || 
00088           !pcl_isfinite (input_->points[(*indices_)[cp]].y) || 
00089           !pcl_isfinite (input_->points[(*indices_)[cp]].z))
00090         continue;
00091 
00092     Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
00093     ijk[0] = static_cast<int> (floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
00094     ijk[1] = static_cast<int> (floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
00095     ijk[2] = static_cast<int> (floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2]));
00096 
00097     // Compute the leaf index
00098     int idx = (ijk - min_b_).dot (divb_mul_);
00099     Leaf& leaf = leaves_[idx];
00100     // First time we initialize the index
00101     if (leaf.idx == -1)
00102     {
00103       leaf.idx = (*indices_)[cp];
00104       continue;
00105     }
00106 
00107     // Check to see if this point is closer to the leaf center than the previous one we saved
00108     float diff_cur   = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
00109     float diff_prev  = (input_->points[leaf.idx].getVector4fMap ()        - ijk.cast<float> ()).squaredNorm ();
00110 
00111     // If current point is closer, copy its index instead
00112     if (diff_cur < diff_prev)
00113       leaf.idx = (*indices_)[cp];
00114   }
00115 
00116   // Second pass: go over all leaves and copy data
00117   output.points.resize (leaves_.size ());
00118   int cp = 0;
00119 
00120   for (typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
00121     output.points[cp++] = it->second.idx;
00122   output.width = static_cast<uint32_t> (output.points.size ());
00123 }
00124 
00125 #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
00126 
00127 #endif    // PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
00128 


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