Point Cloud Library (PCL)  1.8.1-dev
local_maximum.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  * Copyright (c) 2014, RadiantBlue Technologies, Inc.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  * $Id$
39  *
40  */
41 
42 #ifndef PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_
43 #define PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_
44 
45 #include <pcl/common/io.h>
46 #include <pcl/filters/local_maximum.h>
47 #include <pcl/filters/project_inliers.h>
48 #include <pcl/ModelCoefficients.h>
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointT> void
53 {
54  // Has the input dataset been set already?
55  if (!input_)
56  {
57  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
58  output.width = output.height = 0;
59  output.points.clear ();
60  return;
61  }
62 
63  std::vector<int> indices;
64 
65  output.is_dense = true;
66  applyFilterIndices (indices);
67  pcl::copyPointCloud<PointT> (*input_, indices, output);
68 }
69 
70 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71 template <typename PointT> void
73 {
74  typename PointCloud::Ptr cloud_projected (new PointCloud);
75 
76  // Create a set of planar coefficients with X=Y=0,Z=1
78  coefficients->values.resize (4);
79  coefficients->values[0] = coefficients->values[1] = 0;
80  coefficients->values[2] = 1.0;
81  coefficients->values[3] = 0;
82 
83  // Create the filtering object and project input into xy plane
86  proj.setInputCloud (input_);
87  proj.setModelCoefficients (coefficients);
88  proj.filter (*cloud_projected);
89 
90  // Initialize the search class
91  if (!searcher_)
92  {
93  if (input_->isOrganized ())
94  searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
95  else
96  searcher_.reset (new pcl::search::KdTree<PointT> (false));
97  }
98  searcher_->setInputCloud (cloud_projected);
99 
100  // The arrays to be used
101  indices.resize (indices_->size ());
102  removed_indices_->resize (indices_->size ());
103  int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
104 
105  std::vector<bool> point_is_max (indices_->size (), false);
106  std::vector<bool> point_is_visited (indices_->size (), false);
107 
108  // Find all points within xy radius (i.e., a vertical cylinder) of the query
109  // point, removing those that are locally maximal (i.e., highest z within the
110  // cylinder)
111  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
112  {
113  if (!isFinite (input_->points[(*indices_)[iii]]))
114  {
115  continue;
116  }
117 
118  // Points in the neighborhood of a previously identified local max, will
119  // not be maximal in their own neighborhood
120  if (point_is_visited[(*indices_)[iii]] && !point_is_max[(*indices_)[iii]])
121  {
122  continue;
123  }
124 
125  // Assume the current query point is the maximum, mark as visited
126  point_is_max[(*indices_)[iii]] = true;
127  point_is_visited[(*indices_)[iii]] = true;
128 
129  // Perform the radius search in the projected cloud
130  std::vector<int> radius_indices;
131  std::vector<float> radius_dists;
132  PointT p = cloud_projected->points[(*indices_)[iii]];
133  if (searcher_->radiusSearch (p, radius_, radius_indices, radius_dists) == 0)
134  {
135  PCL_WARN ("[pcl::%s::applyFilter] Searching for neighbors within radius %f failed.\n", getClassName ().c_str (), radius_);
136  continue;
137  }
138 
139  // If query point is alone, we retain it regardless
140  if (radius_indices.size () == 1)
141  {
142  point_is_max[(*indices_)[iii]] = false;
143  }
144 
145  // Check to see if a neighbor is higher than the query point
146  float query_z = input_->points[(*indices_)[iii]].z;
147  for (size_t k = 1; k < radius_indices.size (); ++k) // k = 1 is the first neighbor
148  {
149  if (input_->points[radius_indices[k]].z > query_z)
150  {
151  // Query point is not the local max, no need to check others
152  point_is_max[(*indices_)[iii]] = false;
153  break;
154  }
155  }
156 
157  // If the query point was a local max, all neighbors can be marked as
158  // visited, excluding them from future consideration as local maxima
159  if (point_is_max[(*indices_)[iii]])
160  {
161  for (size_t k = 1; k < radius_indices.size (); ++k) // k = 1 is the first neighbor
162  {
163  point_is_visited[radius_indices[k]] = true;
164  }
165  }
166 
167  // Points that are local maxima are passed to removed indices
168  // Unless negative was set, then it's the opposite condition
169  if ((!negative_ && point_is_max[(*indices_)[iii]]) || (negative_ && !point_is_max[(*indices_)[iii]]))
170  {
171  if (extract_removed_indices_)
172  {
173  (*removed_indices_)[rii++] = (*indices_)[iii];
174  }
175 
176  continue;
177  }
178 
179  // Otherwise it was a normal point for output (inlier)
180  indices[oii++] = (*indices_)[iii];
181  }
182 
183  // Resize the output arrays
184  indices.resize (oii);
185  removed_indices_->resize (rii);
186 }
187 
188 #define PCL_INSTANTIATE_LocalMaximum(T) template class PCL_EXPORTS pcl::LocalMaximum<T>;
189 
190 #endif // PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_
191 
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:54
void setModelCoefficients(const ModelCoefficientsConstPtr &model)
Provide a pointer to the model coefficients.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a sepa...
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:132
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
void applyFilter(PointCloud &output)
Downsample a Point Cloud by eliminating points that are locally maximal in z.
boost::shared_ptr< ::pcl::ModelCoefficients > Ptr
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:62
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setModelType(int model)
The type of model to use (user given parameter).
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415