Point Cloud Library (PCL)  1.11.0-dev
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 #pragma once
39 
40 #include <pcl/segmentation/segment_differences.h>
41 
42 #include <pcl/common/io.h>
43 #include <pcl/common/point_tests.h> // for pcl::isFinite
44 
45 
46 //////////////////////////////////////////////////////////////////////////
47 template <typename PointT> void
49  const pcl::PointCloud<PointT> &src,
50  double threshold,
51  const typename pcl::search::Search<PointT>::Ptr &tree,
53 {
54  // We're interested in a single nearest neighbor only
55  std::vector<int> nn_indices (1);
56  std::vector<float> nn_distances (1);
57 
58  // The input cloud indices that do not have a neighbor in the target cloud
59  std::vector<int> src_indices;
60 
61  // Iterate through the source data set
62  for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
63  {
64  // Ignore invalid points in the inpout cloud
65  if (!isFinite (src.points[i]))
66  continue;
67  // Search for the closest point in the target data set (number of neighbors to find = 1)
68  if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
69  {
70  PCL_WARN ("No neighbor found for point %lu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
71  continue;
72  }
73  // Add points without a corresponding point in the target cloud to the output cloud
74  if (nn_distances[0] > threshold)
75  src_indices.push_back (i);
76  }
77 
78  // Copy all the data fields from the input cloud to the output one
79  copyPointCloud (src, src_indices, output);
80 
81  // Output is always dense, as invalid points in the input cloud are ignored
82  output.is_dense = true;
83 }
84 
85 //////////////////////////////////////////////////////////////////////////
86 //////////////////////////////////////////////////////////////////////////
87 //////////////////////////////////////////////////////////////////////////
88 template <typename PointT> void
90 {
91  output.header = input_->header;
92 
93  if (!initCompute ())
94  {
95  output.width = output.height = 0;
96  output.points.clear ();
97  return;
98  }
99 
100  // If target is empty, input - target = input
101  if (target_->points.empty ())
102  {
103  output = *input_;
104  return;
105  }
106 
107  // Initialize the spatial locator
108  if (!tree_)
109  {
110  if (target_->isOrganized ())
111  tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
112  else
113  tree_.reset (new pcl::search::KdTree<PointT> (false));
114  }
115  // Send the input dataset to the spatial locator
116  tree_->setInputCloud (target_);
117 
118  getPointCloudDifference (*input_, distance_threshold_, tree_, output);
119 
120  deinitCompute ();
121 }
122 
123 #define PCL_INSTANTIATE_SegmentDifferences(T) template class PCL_EXPORTS pcl::SegmentDifferences<T>;
124 #define PCL_INSTANTIATE_getPointCloudDifference(T) template PCL_EXPORTS void pcl::getPointCloudDifference<T>(const pcl::PointCloud<T> &, double, const typename pcl::search::Search<T>::Ptr &, pcl::PointCloud<T> &);
125 
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
pcl::SegmentDifferences::segment
void segment(PointCloud &output)
Segment differences between two input point clouds.
Definition: segment_differences.hpp:89
pcl::search::Search::nearestKSearch
virtual int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const =0
Search for the k-nearest neighbors for the given query point.
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:53
pcl::copyPointCloud
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:121
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::search::KdTree< PointT >
pcl::search::Search::Ptr
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:419
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:408
pcl::search::OrganizedNeighbor
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:63
pcl::getPointCloudDifference
void getPointCloudDifference(const pcl::PointCloud< PointT > &src, double threshold, const typename pcl::search::Search< PointT >::Ptr &tree, pcl::PointCloud< PointT > &output)
Obtain the difference between two aligned point clouds as another point cloud, given a distance thres...
Definition: segment_differences.hpp:48