Point Cloud Library (PCL) 1.12.1
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 <queue>
44#include <vector>
45
46namespace pcl
47{
48
49 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50 /** \brief @b OrganizedNeighborSearch class
51 * \note This class provides neighbor search routines for organized point clouds.
52 * \note
53 * \note typename: PointT: type of point used in pointcloud
54 * \author Julius Kammerl (julius@kammerl.de)
55 */
56 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57 template<typename PointT>
59
60 {
61 public:
62
63 /** \brief OrganizedNeighborSearch constructor.
64 * */
68 {
69 max_distance_ = std::numeric_limits<double>::max ();
70
71 focalLength_ = 1.0f;
72 }
73
74 /** \brief Empty deconstructor. */
75 virtual
77 {
78 }
79
80 // public typedefs
84
85
86 /** \brief Provide a pointer to the input data set.
87 * \param cloud_arg the const boost shared pointer to a PointCloud message
88 */
89 inline void
91 {
92
93 if (input_ != cloud_arg)
94 {
95 input_ = cloud_arg;
96
98 generateRadiusLookupTable (input_->width, input_->height);
99 }
100 }
101
102 /** \brief Search for all neighbors of query point that are within a given radius.
103 * \param cloud_arg the point cloud data
104 * \param index_arg the index in \a cloud representing the query point
105 * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
106 * \param k_indices_arg the resultant indices of the neighboring points
107 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
108 * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
109 * \return number of neighbors found in radius
110 */
111 int
112 radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg,
113 std::vector<int> &k_indices_arg, std::vector<float> &k_sqr_distances_arg,
114 int max_nn_arg = INT_MAX);
115
116 /** \brief Search for all neighbors of query point that are within a given radius.
117 * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
118 * If indices were given in setInputCloud, index will be the position in the indices vector
119 * \param radius_arg radius of the sphere bounding all of p_q's neighbors
120 * \param k_indices_arg the resultant indices of the neighboring points
121 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
122 * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
123 * \return number of neighbors found in radius
124 */
125 int
126 radiusSearch (int index_arg, const double radius_arg, std::vector<int> &k_indices_arg,
127 std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
128
129 /** \brief Search for all neighbors of query point that are within a given radius.
130 * \param p_q_arg the given query point
131 * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
132 * \param k_indices_arg the resultant indices of the neighboring points
133 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
134 * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
135 * \return number of neighbors found in radius
136 */
137 int
138 radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector<int> &k_indices_arg,
139 std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
140
141 /** \brief Search for k-nearest neighbors at the query point.
142 * \param cloud_arg the point cloud data
143 * \param index_arg the index in \a cloud representing the query point
144 * \param k_arg the number of neighbors to search for
145 * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
146 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
147 * a priori!)
148 * \return number of neighbors found
149 */
150 int
151 nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector<int> &k_indices_arg,
152 std::vector<float> &k_sqr_distances_arg);
153
154 /** \brief Search for k-nearest neighbors at query point
155 * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
156 * If indices were given in setInputCloud, index will be the position in the indices vector.
157 * \param k_arg the number of neighbors to search for
158 * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
159 * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
160 * a priori!)
161 * \return number of neighbors found
162 */
163 int
164 nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
165 std::vector<float> &k_sqr_distances_arg);
166
167 /** \brief Search for k-nearest neighbors at given query point.
168 * @param p_q_arg the given query point
169 * @param k_arg the number of neighbors to search for
170 * @param k_indices_arg the resultant indices of the neighboring points (must be resized to k a priori!)
171 * @param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to k a priori!)
172 * @return number of neighbors found
173 */
174 int
175 nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
176 std::vector<float> &k_sqr_distances_arg);
177
178 /** \brief Get the maximum allowed distance between the query point and its nearest neighbors. */
179 inline double
181 {
182 return (max_distance_);
183 }
184
185 /** \brief Set the maximum allowed distance between the query point and its nearest neighbors. */
186 inline void
187 setMaxDistance (double max_dist)
188 {
189 max_distance_ = max_dist;
190 }
191
192 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
193 // Protected methods
194 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
195
196 protected:
197
198 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
199 /** \brief @b radiusSearchLoopkupEntry entry for radius search lookup vector
200 * \note This class defines entries for the radius search lookup vector
201 * \author Julius Kammerl (julius@kammerl.de)
202 */
203 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
205 {
206 public:
207
208 /** \brief Empty constructor */
210 {
211 }
212
213 /** \brief Empty deconstructor */
215 {
216 }
217
218 /** \brief Define search point and calculate squared distance
219 * @param x_shift shift in x dimension
220 * @param y_shift shift in y dimension
221 */
222 void
223 defineShiftedSearchPoint(int x_shift, int y_shift)
224 {
225 x_diff_ =x_shift;
226 y_diff_ =y_shift;
227
229 }
230
231 /** \brief Operator< for comparing radiusSearchLoopkupEntry instances with each other. */
232 bool
233 operator< (const radiusSearchLoopkupEntry& rhs_arg) const
234 {
235 return (this->squared_distance_ < rhs_arg.squared_distance_);
236 }
237
238 // Public globals
242
243 };
244
245 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
246 /** \brief @b nearestNeighborCandidate entry for the nearest neighbor candidate queue
247 * \note This class defines entries for the nearest neighbor candidate queue
248 * \author Julius Kammerl (julius@kammerl.de)
249 */
250 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
251
253 {
254 public:
255
256 /** \brief Empty constructor */
258 {
259 }
260
261 /** \brief Empty deconstructor */
263 {
264 }
265
266 /** \brief Operator< for comparing nearestNeighborCandidate instances with each other. */
267 bool
268 operator< (const nearestNeighborCandidate& rhs_arg) const
269 {
270 return (this->squared_distance_ < rhs_arg.squared_distance_);
271 }
272
273 // Public globals
276
277 };
278
279 /** \brief Get point at index from input pointcloud dataset
280 * \param index_arg index representing the point in the dataset given by \a setInputCloud
281 * \return PointT from input pointcloud dataset
282 */
283 const PointT&
284 getPointByIndex (const unsigned int index_arg) const;
285
286 /** \brief Generate radius lookup table. It is used to subsequentially iterate over points
287 * which are close to the search point
288 * \param width of organized point cloud
289 * \param height of organized point cloud
290 */
291 void
292 generateRadiusLookupTable (unsigned int width, unsigned int height);
293
294 inline void
295 pointPlaneProjection (const PointT& point, int& xpos, int& ypos) const
296 {
297 xpos = (int) pcl_round(point.x / (point.z * focalLength_));
298 ypos = (int) pcl_round(point.y / (point.z * focalLength_));
299 }
300
301 void
302 getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& minY_arg, int& maxX_arg, int& maxY_arg ) const;
303
304
305 /** \brief Estimate focal length parameter that was used during point cloud generation
306 */
307 void
309
310 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
311 /** \brief Class getName method. */
312 virtual std::string
313 getName () const
314 {
315 return ("Organized_Neighbor_Search");
316 }
317
318 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
319 // Globals
320 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
321
322 /** \brief Pointer to input point cloud dataset. */
324
325 /** \brief Maximum allowed distance between the query point and its k-neighbors. */
327
328 /** \brief Global focal length parameter */
330
331 /** \brief Precalculated radius search lookup vector */
332 std::vector<radiusSearchLoopkupEntry> radiusSearchLookup_;
335
336 };
337
338}
339
340//#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 deconstructor
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 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=INT_MAX)
Search for all neighbors of query point that are within a given radius.
virtual ~OrganizedNeighborSearch()
Empty deconstructor.
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.
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.
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.