Point Cloud Library (PCL)  1.3.1
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 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: segment_differences.hpp 3035 2011-11-01 04:29:18Z rusu $
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> &tgt, 
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 (size_t i = 0; i < src.points.size (); ++i)
00061   {
00062     // Search for the closest point in the target data set (number of neighbors to find = 1)
00063     if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
00064     {
00065       PCL_WARN ("No neighbor found for point %lu (%f %f %f)!\n", (unsigned long)i, src.points[i].x, src.points[i].y, src.points[i].z);
00066       continue;
00067     }
00068 
00069     if (nn_distances[0] > threshold)
00070       src_indices.push_back (i);
00071   }
00072  
00073   // Allocate enough space and copy the basics
00074   output.points.resize (src_indices.size ());
00075   output.header   = src.header;
00076   output.width    = src_indices.size ();
00077   output.height   = 1;
00078   if (src.is_dense)
00079     output.is_dense = true;
00080   else
00081     // It's not necessarily true that is_dense is false if cloud_in.is_dense is false
00082     // To verify this, we would need to iterate over all points and check for NaNs
00083     output.is_dense = false;
00084 
00085   // Copy all the data fields from the input cloud to the output one
00086   typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00087   // Iterate over each point
00088   for (size_t i = 0; i < src_indices.size (); ++i)
00089     // Iterate over each dimension
00090     pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (src.points[src_indices[i]], output.points[i]));
00091 }
00092 
00096 template <typename PointT> void 
00097 pcl::SegmentDifferences<PointT>::segment (PointCloud &output)
00098 {
00099   output.header = input_->header;
00100 
00101   if (!initCompute ()) 
00102   {
00103     output.width = output.height = 0;
00104     output.points.clear ();
00105     return;
00106   }
00107 
00108   // If target is empty, input - target = input
00109   if (target_->points.empty ())
00110   {
00111     output = *input_;
00112     return;
00113   }
00114 
00115   // Initialize the spatial locator
00116   if (!tree_)
00117   {
00118     if (target_->isOrganized ())
00119       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00120     else
00121       tree_.reset (new pcl::search::KdTree<PointT> (false));
00122   }
00123   // Send the input dataset to the spatial locator
00124   tree_->setInputCloud (target_);
00125 
00126   getPointCloudDifference (*input_, *target_, distance_threshold_, tree_, output);
00127 
00128   deinitCompute ();
00129 }
00130 
00131 #define PCL_INSTANTIATE_SegmentDifferences(T) template class PCL_EXPORTS pcl::SegmentDifferences<T>;
00132 #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> &);
00133 
00134 #endif        // PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
00135 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines