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