Point Cloud Library (PCL) 1.13.0
octree_search.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 */
38
39#pragma once
40
41#include <pcl/octree/octree_pointcloud.h>
42#include <pcl/point_cloud.h>
43
44namespace pcl {
45namespace octree {
46
47/** \brief @b Octree pointcloud search class
48 * \note This class provides several methods for spatial neighbor search based on octree
49 * structure
50 * \tparam PointT type of point used in pointcloud
51 * \ingroup octree
52 * \author Julius Kammerl (julius@kammerl.de)
53 */
54template <typename PointT,
55 typename LeafContainerT = OctreeContainerPointIndices,
56 typename BranchContainerT = OctreeContainerEmpty>
58: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
59public:
60 // public typedefs
61 using IndicesPtr = shared_ptr<Indices>;
62 using IndicesConstPtr = shared_ptr<const Indices>;
63
67
68 // Boost shared pointers
69 using Ptr =
70 shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
71 using ConstPtr = shared_ptr<
73
74 // Eigen aligned allocator
75 using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT>>;
76
78 using LeafNode = typename OctreeT::LeafNode;
80
81 /** \brief Constructor.
82 * \param[in] resolution octree resolution at lowest octree level
83 */
84 OctreePointCloudSearch(const double resolution)
85 : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution)
86 {}
87
88 /** \brief Search for neighbors within a voxel at given point
89 * \param[in] point point addressing a leaf node voxel
90 * \param[out] point_idx_data the resultant indices of the neighboring voxel points
91 * \return "true" if leaf node exist; "false" otherwise
92 */
93 bool
94 voxelSearch(const PointT& point, Indices& point_idx_data);
95
96 /** \brief Search for neighbors within a voxel at given point referenced by a point
97 * index
98 * \param[in] index the index in input cloud defining the query point
99 * \param[out] point_idx_data the resultant indices of the neighboring voxel points
100 * \return "true" if leaf node exist; "false" otherwise
101 */
102 bool
103 voxelSearch(uindex_t index, Indices& point_idx_data);
104
105 /** \brief Search for k-nearest neighbors at the query point.
106 * \param[in] cloud the point cloud data
107 * \param[in] index the index in \a cloud representing the query point
108 * \param[in] k the number of neighbors to search for
109 * \param[out] k_indices the resultant indices of the neighboring points (must be
110 * resized to \a k a priori!)
111 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
112 * points (must be resized to \a k a priori!)
113 * \return number of neighbors found
114 */
115 inline uindex_t
117 uindex_t index,
118 uindex_t k,
119 Indices& k_indices,
120 std::vector<float>& k_sqr_distances)
121 {
122 return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
123 }
124
125 /** \brief Search for k-nearest neighbors at given query point.
126 * \param[in] p_q the given query point
127 * \param[in] k the number of neighbors to search for
128 * \param[out] k_indices the resultant indices of the neighboring points (must be
129 * resized to k a priori!)
130 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
131 * points (must be resized to k a priori!)
132 * \return number of neighbors found
133 */
135 nearestKSearch(const PointT& p_q,
136 uindex_t k,
137 Indices& k_indices,
138 std::vector<float>& k_sqr_distances);
139
140 /** \brief Search for k-nearest neighbors at query point
141 * \param[in] index index representing the query point in the dataset given by \a
142 * setInputCloud. If indices were given in setInputCloud, index will be the position
143 * in the indices vector.
144 * \param[in] k the number of neighbors to search for
145 * \param[out] k_indices the resultant indices of the neighboring points (must be
146 * resized to \a k a priori!)
147 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
148 * points (must be resized to \a k a priori!)
149 * \return number of neighbors found
150 */
153 uindex_t k,
154 Indices& k_indices,
155 std::vector<float>& k_sqr_distances);
156
157 /** \brief Search for approx. nearest neighbor at the query point.
158 * \param[in] cloud the point cloud data
159 * \param[in] query_index the index in \a cloud representing the query point
160 * \param[out] result_index the resultant index of the neighbor point
161 * \param[out] sqr_distance the resultant squared distance to the neighboring point
162 * \return number of neighbors found
163 */
164 inline void
166 uindex_t query_index,
167 index_t& result_index,
168 float& sqr_distance)
169 {
170 return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
171 }
172
173 /** \brief Search for approx. nearest neighbor at the query point.
174 * \param[in] p_q the given query point
175 * \param[out] result_index the resultant index of the neighbor point
176 * \param[out] sqr_distance the resultant squared distance to the neighboring point
177 */
178 void
179 approxNearestSearch(const PointT& p_q, index_t& result_index, float& sqr_distance);
180
181 /** \brief Search for approx. nearest neighbor at the query point.
182 * \param[in] query_index index representing the query point in the dataset given by
183 * \a setInputCloud. If indices were given in setInputCloud, index will be the
184 * position in the indices vector.
185 * \param[out] result_index the resultant index of the neighbor point
186 * \param[out] sqr_distance the resultant squared distance to the neighboring point
187 * \return number of neighbors found
188 */
189 void
190 approxNearestSearch(uindex_t query_index, index_t& result_index, float& sqr_distance);
191
192 /** \brief Search for all neighbors of query point that are within a given radius.
193 * \param[in] cloud the point cloud data
194 * \param[in] index the index in \a cloud representing the query point
195 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
196 * \param[out] k_indices the resultant indices of the neighboring points
197 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
198 * points
199 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
200 * \return number of neighbors found in radius
201 */
204 uindex_t index,
205 double radius,
206 Indices& k_indices,
207 std::vector<float>& k_sqr_distances,
208 index_t max_nn = 0)
209 {
210 return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
211 }
212
213 /** \brief Search for all neighbors of query point that are within a given radius.
214 * \param[in] p_q the given query point
215 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
216 * \param[out] k_indices the resultant indices of the neighboring points
217 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
218 * points
219 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
220 * \return number of neighbors found in radius
221 */
223 radiusSearch(const PointT& p_q,
224 const double radius,
225 Indices& k_indices,
226 std::vector<float>& k_sqr_distances,
227 uindex_t max_nn = 0) const;
228
229 /** \brief Search for all neighbors of query point that are within a given radius.
230 * \param[in] index index representing the query point in the dataset given by \a
231 * setInputCloud. If indices were given in setInputCloud, index will be the position
232 * in the indices vector
233 * \param[in] radius radius of the sphere bounding all of p_q's neighbors
234 * \param[out] k_indices the resultant indices of the neighboring points
235 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
236 * points
237 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
238 * \return number of neighbors found in radius
239 */
242 const double radius,
243 Indices& k_indices,
244 std::vector<float>& k_sqr_distances,
245 uindex_t max_nn = 0) const;
246
247 /** \brief Get a PointT vector of centers of all voxels that intersected by a ray
248 * (origin, direction).
249 * \param[in] origin ray origin
250 * \param[in] direction ray direction vector
251 * \param[out] voxel_center_list results are written to this vector of PointT elements
252 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
253 * disable)
254 * \return number of intersected voxels
255 */
257 getIntersectedVoxelCenters(Eigen::Vector3f origin,
258 Eigen::Vector3f direction,
259 AlignedPointTVector& voxel_center_list,
260 uindex_t max_voxel_count = 0) const;
261
262 /** \brief Get indices of all voxels that are intersected by a ray (origin,
263 * direction).
264 * \param[in] origin ray origin
265 * \param[in] direction ray direction vector
266 * \param[out] k_indices resulting point indices from intersected voxels
267 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
268 * disable)
269 * \return number of intersected voxels
270 */
272 getIntersectedVoxelIndices(Eigen::Vector3f origin,
273 Eigen::Vector3f direction,
274 Indices& k_indices,
275 uindex_t max_voxel_count = 0) const;
276
277 /** \brief Search for points within rectangular search area
278 * Points exactly on the edges of the search rectangle are included.
279 * \param[in] min_pt lower corner of search area
280 * \param[in] max_pt upper corner of search area
281 * \param[out] k_indices the resultant point indices
282 * \return number of points found within search area
283 */
285 boxSearch(const Eigen::Vector3f& min_pt,
286 const Eigen::Vector3f& max_pt,
287 Indices& k_indices) const;
288
289protected:
290 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
291 // Octree-based search routines & helpers
292 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
293 /** \brief @b Priority queue entry for branch nodes
294 * \note This class defines priority queue entries for the nearest neighbor search.
295 * \author Julius Kammerl (julius@kammerl.de)
296 */
298 public:
299 /** \brief Empty constructor */
301
302 /** \brief Constructor for initializing priority queue entry.
303 * \param _node pointer to octree node
304 * \param _key octree key addressing voxel in octree structure
305 * \param[in] _point_distance distance of query point to voxel center
306 */
307 prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
308 : node(_node), point_distance(_point_distance), key(_key)
309 {}
310
311 /** \brief Operator< for comparing priority queue entries with each other.
312 * \param[in] rhs the priority queue to compare this against
313 */
314 bool
315 operator<(const prioBranchQueueEntry rhs) const
316 {
317 return (this->point_distance > rhs.point_distance);
318 }
319
320 /** \brief Pointer to octree node. */
322
323 /** \brief Distance to query point. */
325
326 /** \brief Octree key. */
328 };
329
330 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
331 /** \brief @b Priority queue entry for point candidates
332 * \note This class defines priority queue entries for the nearest neighbor point
333 * candidates.
334 * \author Julius Kammerl (julius@kammerl.de)
335 */
337 public:
338 /** \brief Empty constructor */
340
341 /** \brief Constructor for initializing priority queue entry.
342 * \param[in] point_idx index for a dataset point given by \a setInputCloud
343 * \param[in] point_distance distance of query point to voxel center
344 */
345 prioPointQueueEntry(uindex_t point_idx, float point_distance)
346 : point_idx_(point_idx), point_distance_(point_distance)
347 {}
348
349 /** \brief Operator< for comparing priority queue entries with each other.
350 * \param[in] rhs priority queue to compare this against
351 */
352 bool
353 operator<(const prioPointQueueEntry& rhs) const
354 {
355 return (this->point_distance_ < rhs.point_distance_);
356 }
357
358 /** \brief Index representing a point in the dataset given by \a setInputCloud. */
360
361 /** \brief Distance to query point. */
363 };
364
365 /** \brief Helper function to calculate the squared distance between two points
366 * \param[in] point_a point A
367 * \param[in] point_b point B
368 * \return squared distance between point A and point B
369 */
370 float
371 pointSquaredDist(const PointT& point_a, const PointT& point_b) const;
372
373 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
374 // Recursive search routine methods
375 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
376
377 /** \brief Recursive search method that explores the octree and finds neighbors within
378 * a given radius
379 * \param[in] point query point
380 * \param[in] radiusSquared squared search radius
381 * \param[in] node current octree node to be explored
382 * \param[in] key octree key addressing a leaf node.
383 * \param[in] tree_depth current depth/level in the octree
384 * \param[out] k_indices vector of indices found to be neighbors of query point
385 * \param[out] k_sqr_distances squared distances of neighbors to query point
386 * \param[in] max_nn maximum of neighbors to be found
387 */
388 void
390 const double radiusSquared,
391 const BranchNode* node,
392 const OctreeKey& key,
393 uindex_t tree_depth,
394 Indices& k_indices,
395 std::vector<float>& k_sqr_distances,
396 uindex_t max_nn) const;
397
398 /** \brief Recursive search method that explores the octree and finds the K nearest
399 * neighbors
400 * \param[in] point query point
401 * \param[in] K amount of nearest neighbors to be found
402 * \param[in] node current octree node to be explored
403 * \param[in] key octree key addressing a leaf node.
404 * \param[in] tree_depth current depth/level in the octree
405 * \param[in] squared_search_radius squared search radius distance
406 * \param[out] point_candidates priority queue of nearest neigbor point candidates
407 * \return squared search radius based on current point candidate set found
408 */
409 double
411 const PointT& point,
412 uindex_t K,
413 const BranchNode* node,
414 const OctreeKey& key,
415 uindex_t tree_depth,
416 const double squared_search_radius,
417 std::vector<prioPointQueueEntry>& point_candidates) const;
418
419 /** \brief Recursive search method that explores the octree and finds the approximate
420 * nearest neighbor
421 * \param[in] point query point
422 * \param[in] node current octree node to be explored
423 * \param[in] key octree key addressing a leaf node.
424 * \param[in] tree_depth current depth/level in the octree
425 * \param[out] result_index result index is written to this reference
426 * \param[out] sqr_distance squared distance to search
427 */
428 void
430 const BranchNode* node,
431 const OctreeKey& key,
432 uindex_t tree_depth,
433 index_t& result_index,
434 float& sqr_distance);
435
436 /** \brief Recursively search the tree for all intersected leaf nodes and return a
437 * vector of voxel centers. This algorithm is based off the paper An Efficient
438 * Parametric Algorithm for Octree Traversal:
439 * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
440 * \param[in] min_x octree nodes X coordinate of lower bounding box corner
441 * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
442 * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
443 * \param[in] max_x octree nodes X coordinate of upper bounding box corner
444 * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
445 * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
446 * \param[in] a number used for voxel child index remapping
447 * \param[in] node current octree node to be explored
448 * \param[in] key octree key addressing a leaf node.
449 * \param[out] voxel_center_list results are written to this vector of PointT elements
450 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
451 * disable)
452 * \return number of voxels found
453 */
456 double min_y,
457 double min_z,
458 double max_x,
459 double max_y,
460 double max_z,
461 unsigned char a,
462 const OctreeNode* node,
463 const OctreeKey& key,
464 AlignedPointTVector& voxel_center_list,
465 uindex_t max_voxel_count) const;
466
467 /** \brief Recursive search method that explores the octree and finds points within a
468 * rectangular search area
469 * \param[in] min_pt lower corner of search area
470 * \param[in] max_pt upper corner of search area
471 * \param[in] node current octree node to be explored
472 * \param[in] key octree key addressing a leaf node.
473 * \param[in] tree_depth current depth/level in the octree
474 * \param[out] k_indices the resultant point indices
475 */
476 void
477 boxSearchRecursive(const Eigen::Vector3f& min_pt,
478 const Eigen::Vector3f& max_pt,
479 const BranchNode* node,
480 const OctreeKey& key,
481 uindex_t tree_depth,
482 Indices& k_indices) const;
483
484 /** \brief Recursively search the tree for all intersected leaf nodes and return a
485 * vector of indices. This algorithm is based off the paper An Efficient Parametric
486 * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
487 * \param[in] min_x octree nodes X coordinate of lower bounding box corner
488 * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
489 * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
490 * \param[in] max_x octree nodes X coordinate of upper bounding box corner
491 * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
492 * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
493 * \param[in] a number used for voxel child index remapping
494 * \param[in] node current octree node to be explored
495 * \param[in] key octree key addressing a leaf node.
496 * \param[out] k_indices resulting indices
497 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
498 * disable)
499 * \return number of voxels found
500 */
503 double min_y,
504 double min_z,
505 double max_x,
506 double max_y,
507 double max_z,
508 unsigned char a,
509 const OctreeNode* node,
510 const OctreeKey& key,
511 Indices& k_indices,
512 uindex_t max_voxel_count) const;
513
514 /** \brief Initialize raytracing algorithm
515 * \param[in] origin ray origin
516 * \param[in] direction ray direction vector
517 * \param[out] min_x octree nodes X coordinate of lower bounding box corner
518 * \param[out] min_y octree nodes Y coordinate of lower bounding box corner
519 * \param[out] min_z octree nodes Z coordinate of lower bounding box corner
520 * \param[out] max_x octree nodes X coordinate of upper bounding box corner
521 * \param[out] max_y octree nodes Y coordinate of upper bounding box corner
522 * \param[out] max_z octree nodes Z coordinate of upper bounding box corner
523 * \param[out] a number used for voxel child index remapping
524 */
525 inline void
526 initIntersectedVoxel(Eigen::Vector3f& origin,
527 Eigen::Vector3f& direction,
528 double& min_x,
529 double& min_y,
530 double& min_z,
531 double& max_x,
532 double& max_y,
533 double& max_z,
534 unsigned char& a) const
535 {
536 // Account for division by zero when direction vector is 0.0
537 const float epsilon = 1e-10f;
538 if (direction.x() == 0.0)
539 direction.x() = epsilon;
540 if (direction.y() == 0.0)
541 direction.y() = epsilon;
542 if (direction.z() == 0.0)
543 direction.z() = epsilon;
544
545 // Voxel childIdx remapping
546 a = 0;
547
548 // Handle negative axis direction vector
549 if (direction.x() < 0.0) {
550 origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
551 origin.x();
552 direction.x() = -direction.x();
553 a |= 4;
554 }
555 if (direction.y() < 0.0) {
556 origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
557 origin.y();
558 direction.y() = -direction.y();
559 a |= 2;
560 }
561 if (direction.z() < 0.0) {
562 origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
563 origin.z();
564 direction.z() = -direction.z();
565 a |= 1;
566 }
567 min_x = (this->min_x_ - origin.x()) / direction.x();
568 max_x = (this->max_x_ - origin.x()) / direction.x();
569 min_y = (this->min_y_ - origin.y()) / direction.y();
570 max_y = (this->max_y_ - origin.y()) / direction.y();
571 min_z = (this->min_z_ - origin.z()) / direction.z();
572 max_z = (this->max_z_ - origin.z()) / direction.z();
573 }
574
575 /** \brief Find first child node ray will enter
576 * \param[in] min_x octree nodes X coordinate of lower bounding box corner
577 * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
578 * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
579 * \param[in] mid_x octree nodes X coordinate of bounding box mid line
580 * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
581 * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
582 * \return the first child node ray will enter
583 */
584 inline int
586 double min_y,
587 double min_z,
588 double mid_x,
589 double mid_y,
590 double mid_z) const
591 {
592 int currNode = 0;
593
594 if (min_x > min_y) {
595 if (min_x > min_z) {
596 // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
597 if (mid_y < min_x)
598 currNode |= 2;
599 if (mid_z < min_x)
600 currNode |= 1;
601 }
602 else {
603 // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
604 if (mid_x < min_z)
605 currNode |= 4;
606 if (mid_y < min_z)
607 currNode |= 2;
608 }
609 }
610 else {
611 if (min_y > min_z) {
612 // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
613 if (mid_x < min_y)
614 currNode |= 4;
615 if (mid_z < min_y)
616 currNode |= 1;
617 }
618 else {
619 // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
620 if (mid_x < min_z)
621 currNode |= 4;
622 if (mid_y < min_z)
623 currNode |= 2;
624 }
625 }
626
627 return currNode;
628 }
629
630 /** \brief Get the next visited node given the current node upper
631 * bounding box corner. This function accepts three float values, and
632 * three int values. The function returns the ith integer where the
633 * ith float value is the minimum of the three float values.
634 * \param[in] x current nodes X coordinate of upper bounding box corner
635 * \param[in] y current nodes Y coordinate of upper bounding box corner
636 * \param[in] z current nodes Z coordinate of upper bounding box corner
637 * \param[in] a next node if exit Plane YZ
638 * \param[in] b next node if exit Plane XZ
639 * \param[in] c next node if exit Plane XY
640 * \return the next child node ray will enter or 8 if exiting
641 */
642 inline int
643 getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
644 {
645 if (x < y) {
646 if (x < z)
647 return a;
648 return c;
649 }
650 if (y < z)
651 return b;
652 return c;
653 }
654};
655} // namespace octree
656} // namespace pcl
657
658#ifdef PCL_NO_PRECOMPILE
659#include <pcl/octree/impl/octree_search.hpp>
660#endif
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
Octree key class
Definition: octree_key.h:54
Abstract octree node class
Definition: octree_nodes.h:59
Octree pointcloud class
typename OctreeT::LeafNode LeafNode
typename OctreeT::BranchNode BranchNode
Priority queue entry for branch nodes
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
OctreeKey key
Octree key.
const OctreeNode * node
Pointer to octree node.
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
prioBranchQueueEntry()
Empty constructor
float point_distance
Distance to query point.
Priority queue entry for point candidates
prioPointQueueEntry(uindex_t point_idx, float point_distance)
Constructor for initializing priority queue entry.
prioPointQueueEntry()
Empty constructor
uindex_t point_idx_
Index representing a point in the dataset given by setInputCloud.
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
float point_distance_
Distance to query point.
Octree pointcloud search class
Definition: octree_search.h:58
typename OctreeT::LeafNode LeafNode
Definition: octree_search.h:78
double getKNearestNeighborRecursive(const PointT &point, uindex_t K, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, const double squared_search_radius, std::vector< prioPointQueueEntry > &point_candidates) const
Recursive search method that explores the octree and finds the K nearest neighbors.
uindex_t getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, Indices &k_indices, uindex_t max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
int getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
Get the next visited node given the current node upper bounding box corner.
uindex_t getIntersectedVoxelIndicesRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, Indices &k_indices, uindex_t max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices.
uindex_t getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, uindex_t max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_search.h:66
bool voxelSearch(const PointT &point, Indices &point_idx_data)
Search for neighbors within a voxel at given point.
shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:72
shared_ptr< Indices > IndicesPtr
Definition: octree_search.h:61
shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:70
uindex_t boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, Indices &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
uindex_t getIntersectedVoxelCentersRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, AlignedPointTVector &voxel_center_list, uindex_t max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, index_t &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor.
uindex_t nearestKSearch(const PointCloud &cloud, uindex_t index, uindex_t k, Indices &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, Indices &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area.
void initIntersectedVoxel(Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
Initialize raytracing algorithm.
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, Indices &k_indices, std::vector< float > &k_sqr_distances, uindex_t max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius.
shared_ptr< const Indices > IndicesConstPtr
Definition: octree_search.h:62
typename OctreeT::BranchNode BranchNode
Definition: octree_search.h:79
void approxNearestSearch(const PointCloud &cloud, uindex_t query_index, index_t &result_index, float &sqr_distance)
Search for approx.
int getFirstIntersectedNode(double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
Find first child node ray will enter.
uindex_t radiusSearch(const PointCloud &cloud, uindex_t index, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, index_t max_nn=0)
Search for all neighbors of query point that are within a given radius.
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:84
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:75
typename PointCloud::Ptr PointCloudPtr
Definition: octree_search.h:65
@ K
Definition: norms.h:54
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.