segment_differences.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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 the copyright holder(s) 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_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
00039 #define PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
00040 
00041 #include <pcl/segmentation/segment_differences.h>
00042 #include <pcl/common/concatenate.h>
00043 
00045 template <typename PointT> void
00046 pcl::getPointCloudDifference (
00047     const pcl::PointCloud<PointT> &src, 
00048     const pcl::PointCloud<PointT> &, 
00049     double threshold, const boost::shared_ptr<pcl::search::Search<PointT> > &tree,
00050     pcl::PointCloud<PointT> &output)
00051 {
00052   // We're interested in a single nearest neighbor only
00053   std::vector<int> nn_indices (1);
00054   std::vector<float> nn_distances (1);
00055 
00056   // The src indices that do not have a neighbor in tgt
00057   std::vector<int> src_indices;
00058 
00059   // Iterate through the source data set
00060   for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
00061   {
00062     if (!isFinite (src.points[i]))
00063       continue;
00064     // Search for the closest point in the target data set (number of neighbors to find = 1)
00065     if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
00066     {
00067       PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
00068       continue;
00069     }
00070 
00071     if (nn_distances[0] > threshold)
00072       src_indices.push_back (i);
00073   }
00074  
00075   // Allocate enough space and copy the basics
00076   output.points.resize (src_indices.size ());
00077   output.header   = src.header;
00078   output.width    = static_cast<uint32_t> (src_indices.size ());
00079   output.height   = 1;
00080   //if (src.is_dense)
00081     output.is_dense = true;
00082   //else
00083     // It's not necessarily true that is_dense is false if cloud_in.is_dense is false
00084     // To verify this, we would need to iterate over all points and check for NaNs
00085     //output.is_dense = false;
00086 
00087   // Copy all the data fields from the input cloud to the output one
00088   typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00089   // Iterate over each point
00090   for (size_t i = 0; i < src_indices.size (); ++i)
00091     // Iterate over each dimension
00092     pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (src.points[src_indices[i]], output.points[i]));
00093 }
00094 
00098 template <typename PointT> void 
00099 pcl::SegmentDifferences<PointT>::segment (PointCloud &output)
00100 {
00101   output.header = input_->header;
00102 
00103   if (!initCompute ()) 
00104   {
00105     output.width = output.height = 0;
00106     output.points.clear ();
00107     return;
00108   }
00109 
00110   // If target is empty, input - target = input
00111   if (target_->points.empty ())
00112   {
00113     output = *input_;
00114     return;
00115   }
00116 
00117   // Initialize the spatial locator
00118   if (!tree_)
00119   {
00120     if (target_->isOrganized ())
00121       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00122     else
00123       tree_.reset (new pcl::search::KdTree<PointT> (false));
00124   }
00125   // Send the input dataset to the spatial locator
00126   tree_->setInputCloud (target_);
00127 
00128   getPointCloudDifference (*input_, *target_, distance_threshold_, tree_, output);
00129 
00130   deinitCompute ();
00131 }
00132 
00133 #define PCL_INSTANTIATE_SegmentDifferences(T) template class PCL_EXPORTS pcl::SegmentDifferences<T>;
00134 #define PCL_INSTANTIATE_getPointCloudDifference(T) template PCL_EXPORTS void pcl::getPointCloudDifference<T>(const pcl::PointCloud<T> &, const pcl::PointCloud<T> &, double, const boost::shared_ptr<pcl::search::Search<T> > &, pcl::PointCloud<T> &);
00135 
00136 #endif        // PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
00137 


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