Point Cloud Library (PCL) 1.13.0
organized_neighbor_search.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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 Willow Garage, Inc. 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 * Author: Julius Kammerl (julius@kammerl.de)
35 */
36
37#pragma once
38
39#include <pcl/point_cloud.h>
40#include <pcl/point_types.h>
41
42#include <algorithm>
43#include <limits>
44#include <queue>
45#include <vector>
46
47namespace pcl
48{
49
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 /** \brief @b OrganizedNeighborSearch class
52 * \note This class provides neighbor search routines for organized point clouds.
53 * \note
54 * \note typename: PointT: type of point used in pointcloud
55 * \author Julius Kammerl (julius@kammerl.de)
56 */
57 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
58 template<typename PointT>
60
61 {
62 public:
63
64 /** \brief OrganizedNeighborSearch constructor.
65 * */
69 {
70 max_distance_ = std::numeric_limits<double>::max ();
71
72 focalLength_ = 1.0f;
73 }
74
75 /** \brief Empty deconstructor. */
76 virtual
78
79 // public typedefs
83
84
85 /** \brief Provide a pointer to the input data set.
86 * \param cloud_arg the const boost shared pointer to a PointCloud message
87 */
88 inline void
90 {
91
92 if (input_ != cloud_arg)
93 {
94 input_ = cloud_arg;
95
97 generateRadiusLookupTable (input_->width, input_->height);
98 }
99 }
100
101 /** \brief Search for all neighbors of query point that are within a given radius.
102 * \param cloud_arg the point cloud data
103 * \param index_arg the index in \a cloud representing the query point
104 * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
105 * \param k_indices_arg the resultant indices of the neighboring points
106 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
107 * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
108 * \return number of neighbors found in radius
109 */
110 int
111 radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg,
112 std::vector<int> &k_indices_arg, std::vector<float> &k_sqr_distances_arg,
113 int max_nn_arg = std::numeric_limits<int>::max());
114
115 /** \brief Search for all neighbors of query point that are within a given radius.
116 * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
117 * If indices were given in setInputCloud, index will be the position in the indices vector
118 * \param radius_arg radius of the sphere bounding all of p_q's neighbors
119 * \param k_indices_arg the resultant indices of the neighboring points
120 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
121 * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
122 * \return number of neighbors found in radius
123 */
124 int
125 radiusSearch (int index_arg, const double radius_arg, std::vector<int> &k_indices_arg,
126 std::vector<float> &k_sqr_distances_arg, int max_nn_arg = std::numeric_limits<int>::max()) const;
127
128 /** \brief Search for all neighbors of query point that are within a given radius.
129 * \param p_q_arg the given query point
130 * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
131 * \param k_indices_arg the resultant indices of the neighboring points
132 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
133 * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
134 * \return number of neighbors found in radius
135 */
136 int
137 radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector<int> &k_indices_arg,
138 std::vector<float> &k_sqr_distances_arg, int max_nn_arg = std::numeric_limits<int>::max()) const;
139
140 /** \brief Search for k-nearest neighbors at the query point.
141 * \param cloud_arg the point cloud data
142 * \param index_arg the index in \a cloud representing the query point
143 * \param k_arg the number of neighbors to search for
144 * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
145 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
146 * a priori!)
147 * \return number of neighbors found
148 */
149 int
150 nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector<int> &k_indices_arg,
151 std::vector<float> &k_sqr_distances_arg);
152
153 /** \brief Search for k-nearest neighbors at query point
154 * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
155 * If indices were given in setInputCloud, index will be the position in the indices vector.
156 * \param k_arg the number of neighbors to search for
157 * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
158 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
159 * a priori!)
160 * \return number of neighbors found
161 */
162 int
163 nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
164 std::vector<float> &k_sqr_distances_arg);
165
166 /** \brief Search for k-nearest neighbors at given query point.
167 * @param p_q_arg the given query point
168 * @param k_arg the number of neighbors to search for
169 * @param k_indices_arg the resultant indices of the neighboring points (must be resized to k a priori!)
170 * @param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to k a priori!)
171 * @return number of neighbors found
172 */
173 int
174 nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
175 std::vector<float> &k_sqr_distances_arg);
176
177 /** \brief Get the maximum allowed distance between the query point and its nearest neighbors. */
178 inline double
180 {
181 return (max_distance_);
182 }
183
184 /** \brief Set the maximum allowed distance between the query point and its nearest neighbors. */
185 inline void
186 setMaxDistance (double max_dist)
187 {
188 max_distance_ = max_dist;
189 }
190
191 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
192 // Protected methods
193 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
194
195 protected:
196
197 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
198 /** \brief @b radiusSearchLoopkupEntry entry for radius search lookup vector
199 * \note This class defines entries for the radius search lookup vector
200 * \author Julius Kammerl (julius@kammerl.de)
201 */
202 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
204 {
205 public:
206
207 /** \brief Empty constructor */
209 {
210 }
211
212 /** \brief Define search point and calculate squared distance
213 * @param x_shift shift in x dimension
214 * @param y_shift shift in y dimension
215 */
216 void
217 defineShiftedSearchPoint(int x_shift, int y_shift)
218 {
219 x_diff_ =x_shift;
220 y_diff_ =y_shift;
221
223 }
224
225 /** \brief Operator< for comparing radiusSearchLoopkupEntry instances with each other. */
226 bool
227 operator< (const radiusSearchLoopkupEntry& rhs_arg) const
228 {
229 return (this->squared_distance_ < rhs_arg.squared_distance_);
230 }
231
232 // Public globals
236
237 };
238
239 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
240 /** \brief @b nearestNeighborCandidate entry for the nearest neighbor candidate queue
241 * \note This class defines entries for the nearest neighbor candidate queue
242 * \author Julius Kammerl (julius@kammerl.de)
243 */
244 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
245
247 {
248 public:
249
250 /** \brief Empty constructor */
252 {
253 }
254
255 /** \brief Operator< for comparing nearestNeighborCandidate instances with each other. */
256 bool
257 operator< (const nearestNeighborCandidate& rhs_arg) const
258 {
259 return (this->squared_distance_ < rhs_arg.squared_distance_);
260 }
261
262 // Public globals
265
266 };
267
268 /** \brief Get point at index from input pointcloud dataset
269 * \param index_arg index representing the point in the dataset given by \a setInputCloud
270 * \return PointT from input pointcloud dataset
271 */
272 const PointT&
273 getPointByIndex (const unsigned int index_arg) const;
274
275 /** \brief Generate radius lookup table. It is used to subsequentially iterate over points
276 * which are close to the search point
277 * \param width of organized point cloud
278 * \param height of organized point cloud
279 */
280 void
281 generateRadiusLookupTable (unsigned int width, unsigned int height);
282
283 inline void
284 pointPlaneProjection (const PointT& point, int& xpos, int& ypos) const
285 {
286 xpos = (int) pcl_round(point.x / (point.z * focalLength_));
287 ypos = (int) pcl_round(point.y / (point.z * focalLength_));
288 }
289
290 void
291 getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& minY_arg, int& maxX_arg, int& maxY_arg ) const;
292
293
294 /** \brief Estimate focal length parameter that was used during point cloud generation
295 */
296 void
298
299 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
300 /** \brief Class getName method. */
301 virtual std::string
302 getName () const
303 {
304 return ("Organized_Neighbor_Search");
305 }
306
307 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
308 // Globals
309 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
310
311 /** \brief Pointer to input point cloud dataset. */
313
314 /** \brief Maximum allowed distance between the query point and its k-neighbors. */
316
317 /** \brief Global focal length parameter */
319
320 /** \brief Precalculated radius search lookup vector */
321 std::vector<radiusSearchLoopkupEntry> radiusSearchLookup_;
324
325 };
326
327}
328
329//#include "organized_neighbor_search.hpp"
nearestNeighborCandidate entry for the nearest neighbor candidate queue
bool operator<(const nearestNeighborCandidate &rhs_arg) const
Operator< for comparing nearestNeighborCandidate instances with each other.
radiusSearchLoopkupEntry entry for radius search lookup vector
int squared_distance_
int y_diff_
bool operator<(const radiusSearchLoopkupEntry &rhs_arg) const
Operator< for comparing radiusSearchLoopkupEntry instances with each other.
radiusSearchLoopkupEntry()
Empty constructor
void defineShiftedSearchPoint(int x_shift, int y_shift)
Define search point and calculate squared distance.
int x_diff_
OrganizedNeighborSearch class
PointCloudConstPtr input_
Pointer to input point cloud dataset.
std::vector< radiusSearchLoopkupEntry > radiusSearchLookup_
Precalculated radius search lookup vector.
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const
int nearestKSearch(const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector< int > &k_indices_arg, std::vector< float > &k_sqr_distances_arg)
Search for k-nearest neighbors at the query point.
void setMaxDistance(double max_dist)
Set the maximum allowed distance between the query point and its nearest neighbors.
void estimateFocalLengthFromInputCloud()
Estimate focal length parameter that was used during point cloud generation.
typename PointCloud::ConstPtr PointCloudConstPtr
OrganizedNeighborSearch()
OrganizedNeighborSearch constructor.
double getMaxDistance() const
Get the maximum allowed distance between the query point and its nearest neighbors.
int radiusSearch(const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg, std::vector< int > &k_indices_arg, std::vector< float > &k_sqr_distances_arg, int max_nn_arg=std::numeric_limits< int >::max())
Search for all neighbors of query point that are within a given radius.
virtual std::string getName() const
Class getName method.
typename PointCloud::Ptr PointCloudPtr
double max_distance_
Maximum allowed distance between the query point and its k-neighbors.
virtual ~OrganizedNeighborSearch()=default
Empty deconstructor.
void pointPlaneProjection(const PointT &point, int &xpos, int &ypos) const
double focalLength_
Global focal length parameter.
void generateRadiusLookupTable(unsigned int width, unsigned int height)
Generate radius lookup table.
void setInputCloud(const PointCloudConstPtr &cloud_arg)
Provide a pointer to the input data set.
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Defines all the PCL implemented PointT point type structures.
__inline double pcl_round(double number)
Win32 doesn't seem to have rounding functions.
Definition: pcl_macros.h:239
A point structure representing Euclidean xyz coordinates, and the RGB color.