Point Cloud Library (PCL)
1.3.1
|
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