Point Cloud Library (PCL)  1.7.1
segment_differences.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
39 #define PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
40 
41 #include <pcl/segmentation/segment_differences.h>
42 #include <pcl/common/concatenate.h>
43 
44 //////////////////////////////////////////////////////////////////////////
45 template <typename PointT> void
47  const pcl::PointCloud<PointT> &src,
48  const pcl::PointCloud<PointT> &,
49  double threshold, const boost::shared_ptr<pcl::search::Search<PointT> > &tree,
51 {
52  // We're interested in a single nearest neighbor only
53  std::vector<int> nn_indices (1);
54  std::vector<float> nn_distances (1);
55 
56  // The src indices that do not have a neighbor in tgt
57  std::vector<int> src_indices;
58 
59  // Iterate through the source data set
60  for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
61  {
62  if (!isFinite (src.points[i]))
63  continue;
64  // Search for the closest point in the target data set (number of neighbors to find = 1)
65  if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
66  {
67  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);
68  continue;
69  }
70 
71  if (nn_distances[0] > threshold)
72  src_indices.push_back (i);
73  }
74 
75  // Allocate enough space and copy the basics
76  output.points.resize (src_indices.size ());
77  output.header = src.header;
78  output.width = static_cast<uint32_t> (src_indices.size ());
79  output.height = 1;
80  //if (src.is_dense)
81  output.is_dense = true;
82  //else
83  // It's not necessarily true that is_dense is false if cloud_in.is_dense is false
84  // To verify this, we would need to iterate over all points and check for NaNs
85  //output.is_dense = false;
86 
87  // Copy all the data fields from the input cloud to the output one
88  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
89  // Iterate over each point
90  for (size_t i = 0; i < src_indices.size (); ++i)
91  // Iterate over each dimension
92  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (src.points[src_indices[i]], output.points[i]));
93 }
94 
95 //////////////////////////////////////////////////////////////////////////
96 //////////////////////////////////////////////////////////////////////////
97 //////////////////////////////////////////////////////////////////////////
98 template <typename PointT> void
100 {
101  output.header = input_->header;
102 
103  if (!initCompute ())
104  {
105  output.width = output.height = 0;
106  output.points.clear ();
107  return;
108  }
109 
110  // If target is empty, input - target = input
111  if (target_->points.empty ())
112  {
113  output = *input_;
114  return;
115  }
116 
117  // Initialize the spatial locator
118  if (!tree_)
119  {
120  if (target_->isOrganized ())
121  tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
122  else
123  tree_.reset (new pcl::search::KdTree<PointT> (false));
124  }
125  // Send the input dataset to the spatial locator
126  tree_->setInputCloud (target_);
127 
128  getPointCloudDifference (*input_, *target_, distance_threshold_, tree_, output);
129 
130  deinitCompute ();
131 }
132 
133 #define PCL_INSTANTIATE_SegmentDifferences(T) template class PCL_EXPORTS pcl::SegmentDifferences<T>;
134 #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> &);
135 
136 #endif // PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
137