Point Cloud Library (PCL)  1.15.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 #pragma once
43 
44 #include <pcl/common/io.h>
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 #include <pcl/filters/local_maximum.h>
47 #include <pcl/filters/project_inliers.h>
48 #include <pcl/ModelCoefficients.h>
49 #include <pcl/search/auto.h> // for autoSelectMethod
50 
51 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
52 template <typename PointT> void
54 {
55  // Has the input dataset been set already?
56  if (!input_)
57  {
58  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
59  output.width = output.height = 0;
60  output.clear ();
61  return;
62  }
63 
64  Indices indices;
65 
66  output.is_dense = true;
67  applyFilterIndices (indices);
68  pcl::copyPointCloud<PointT> (*input_, indices, output);
69 }
70 
71 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointT> void
74 {
75  typename PointCloud::Ptr cloud_projected (new PointCloud);
76 
77  // Create a set of planar coefficients with X=Y=0,Z=1
79  coefficients->values.resize (4);
80  coefficients->values[0] = coefficients->values[1] = 0;
81  coefficients->values[2] = 1.0;
82  coefficients->values[3] = 0;
83 
84  // Create the filtering object and project input into xy plane
87  proj.setInputCloud (input_);
88  proj.setModelCoefficients (coefficients);
89  proj.filter (*cloud_projected);
90 
91  // Initialize the search class
92  if (!searcher_)
93  {
94  searcher_.reset (pcl::search::autoSelectMethod<PointT>(cloud_projected, false, pcl::search::Purpose::radius_search));
95  }
96  else if (!searcher_->setInputCloud (cloud_projected))
97  {
98  PCL_ERROR ("[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ());
99  indices.clear ();
100  removed_indices_->clear ();
101  return;
102  }
103 
104  // The arrays to be used
105  indices.resize (indices_->size ());
106  removed_indices_->resize (indices_->size ());
107  int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
108 
109  std::vector<bool> point_is_max (indices_->size (), false);
110  std::vector<bool> point_is_visited (indices_->size (), false);
111 
112  // Find all points within xy radius (i.e., a vertical cylinder) of the query
113  // point, removing those that are locally maximal (i.e., highest z within the
114  // cylinder)
115  for (const auto& iii : (*indices_))
116  {
117  if (!isFinite ((*input_)[iii]))
118  {
119  continue;
120  }
121 
122  // Points in the neighborhood of a previously identified local max, will
123  // not be maximal in their own neighborhood
124  if (point_is_visited[iii] && !point_is_max[iii])
125  {
126  if (negative_) {
127  if (extract_removed_indices_) {
128  (*removed_indices_)[rii++] = iii;
129  }
130  }
131  else {
132  indices[oii++] = iii;
133  }
134  continue;
135  }
136 
137  // Assume the current query point is the maximum, mark as visited
138  point_is_max[iii] = true;
139  point_is_visited[iii] = true;
140 
141  // Perform the radius search in the projected cloud
142  Indices radius_indices;
143  std::vector<float> radius_dists;
144  PointT p = (*cloud_projected)[iii];
145  if (searcher_->radiusSearch (p, radius_, radius_indices, radius_dists) == 0)
146  {
147  PCL_WARN ("[pcl::%s::applyFilter] Searching for neighbors within radius %f failed.\n", getClassName ().c_str (), radius_);
148  continue;
149  }
150 
151  // If query point is alone, we retain it regardless
152  if (radius_indices.size () == 1)
153  {
154  point_is_max[iii] = false;
155  }
156 
157  // Check to see if a neighbor is higher than the query point
158  float query_z = (*input_)[iii].z;
159  for (const auto& radius_index : radius_indices) // the query point itself is in the (unsorted) radius_indices, but that is okay since we compare with ">"
160  {
161  if ((*input_)[radius_index].z > query_z)
162  {
163  // Query point is not the local max, no need to check others
164  point_is_max[iii] = false;
165  break;
166  }
167  }
168 
169  // If the query point was a local max, all neighbors can be marked as
170  // visited, excluding them from future consideration as local maxima
171  if (point_is_max[iii])
172  {
173  for (const auto& radius_index : radius_indices) // the query point itself is in the (unsorted) radius_indices, but it must also be marked as visited
174  {
175  point_is_visited[radius_index] = true;
176  }
177  }
178 
179  // Points that are local maxima are passed to removed indices
180  // Unless negative was set, then it's the opposite condition
181  if ((!negative_ && point_is_max[iii]) || (negative_ && !point_is_max[iii]))
182  {
183  if (extract_removed_indices_)
184  {
185  (*removed_indices_)[rii++] = iii;
186  }
187 
188  continue;
189  }
190 
191  // Otherwise it was a normal point for output (inlier)
192  indices[oii++] = iii;
193  }
194 
195  // Resize the output arrays
196  indices.resize (oii);
197  removed_indices_->resize (rii);
198 }
199 
200 #define PCL_INSTANTIATE_LocalMaximum(T) template class PCL_EXPORTS pcl::LocalMaximum<T>;
201 
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:121
void applyFilter(PointCloud &output) override
Downsample a Point Cloud by eliminating points that are locally maximal in z.
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:174
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:404
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:399
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:401
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:886
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a sepa...
void setModelCoefficients(const ModelCoefficientsConstPtr &model)
Provide a pointer to the model coefficients.
void setModelType(int model)
The type of model to use (user given parameter).
@ radius_search
The search method will mainly be used for radiusSearch.
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
@ SACMODEL_PLANE
Definition: model_types.h:47
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< ::pcl::ModelCoefficients > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.