Point Cloud Library (PCL)
1.7.0
|
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 00044 ////////////////////////////////////////////////////////////////////////// 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 00095 ////////////////////////////////////////////////////////////////////////// 00096 ////////////////////////////////////////////////////////////////////////// 00097 ////////////////////////////////////////////////////////////////////////// 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