Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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 */
163 inline void
165 uindex_t query_index,
166 index_t& result_index,
167 float& sqr_distance)
168 {
169 return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
170 }
171
172 /** \brief Search for approx. nearest neighbor at the query point.
173 * \param[in] p_q the given query point
174 * \param[out] result_index the resultant index of the neighbor point
175 * \param[out] sqr_distance the resultant squared distance to the neighboring point
176 */
177 void
178 approxNearestSearch(const PointT& p_q, index_t& result_index, float& sqr_distance);
179
180 /** \brief Search for approx. nearest neighbor at the query point.
181 * \param[in] query_index index representing the query point in the dataset given by
182 * \a setInputCloud. If indices were given in setInputCloud, index will be the
183 * position in the indices vector.
184 * \param[out] result_index the resultant index of the neighbor point
185 * \param[out] sqr_distance the resultant squared distance to the neighboring point
186 */
187 void
188 approxNearestSearch(uindex_t query_index, index_t& result_index, float& sqr_distance);
189
190 /** \brief Search for all neighbors of query point that are within a given radius.
191 * \param[in] cloud the point cloud data
192 * \param[in] index the index in \a cloud representing the query point
193 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
194 * \param[out] k_indices the resultant indices of the neighboring points
195 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
196 * points
197 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
198 * \return number of neighbors found in radius
199 */
202 uindex_t index,
203 double radius,
204 Indices& k_indices,
205 std::vector<float>& k_sqr_distances,
206 index_t max_nn = 0)
207 {
208 return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
209 }
210
211 /** \brief Search for all neighbors of query point that are within a given radius.
212 * \param[in] p_q the given query point
213 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
214 * \param[out] k_indices the resultant indices of the neighboring points
215 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
216 * points
217 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
218 * \return number of neighbors found in radius
219 */
221 radiusSearch(const PointT& p_q,
222 const double radius,
223 Indices& k_indices,
224 std::vector<float>& k_sqr_distances,
225 uindex_t max_nn = 0) const;
226
227 /** \brief Search for all neighbors of query point that are within a given radius.
228 * \param[in] index index representing the query point in the dataset given by \a
229 * setInputCloud. If indices were given in setInputCloud, index will be the position
230 * in the indices vector
231 * \param[in] radius radius of the sphere bounding all of p_q's neighbors
232 * \param[out] k_indices the resultant indices of the neighboring points
233 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
234 * points
235 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
236 * \return number of neighbors found in radius
237 */
240 const double radius,
241 Indices& k_indices,
242 std::vector<float>& k_sqr_distances,
243 uindex_t max_nn = 0) const;
244
245 /** \brief Get a PointT vector of centers of all voxels that intersected by a ray
246 * (origin, direction).
247 * \param[in] origin ray origin
248 * \param[in] direction ray direction vector
249 * \param[out] voxel_center_list results are written to this vector of PointT elements
250 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
251 * disable)
252 * \return number of intersected voxels
253 */
255 getIntersectedVoxelCenters(Eigen::Vector3f origin,
256 Eigen::Vector3f direction,
257 AlignedPointTVector& voxel_center_list,
258 uindex_t max_voxel_count = 0) const;
259
260 /** \brief Get indices of all voxels that are intersected by a ray (origin,
261 * direction).
262 * \param[in] origin ray origin
263 * \param[in] direction ray direction vector
264 * \param[out] k_indices resulting point indices from intersected voxels
265 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
266 * disable)
267 * \return number of intersected voxels
268 */
270 getIntersectedVoxelIndices(Eigen::Vector3f origin,
271 Eigen::Vector3f direction,
272 Indices& k_indices,
273 uindex_t max_voxel_count = 0) const;
274
275 /** \brief Search for points within rectangular search area
276 * Points exactly on the edges of the search rectangle are included.
277 * \param[in] min_pt lower corner of search area
278 * \param[in] max_pt upper corner of search area
279 * \param[out] k_indices the resultant point indices
280 * \return number of points found within search area
281 */
283 boxSearch(const Eigen::Vector3f& min_pt,
284 const Eigen::Vector3f& max_pt,
285 Indices& k_indices) const;
286
287protected:
288 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
289 // Octree-based search routines & helpers
290 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
291 /** \brief @b Priority queue entry for branch nodes
292 * \note This class defines priority queue entries for the nearest neighbor search.
293 * \author Julius Kammerl (julius@kammerl.de)
294 */
296 public:
297 /** \brief Empty constructor */
299
300 /** \brief Constructor for initializing priority queue entry.
301 * \param _node pointer to octree node
302 * \param _key octree key addressing voxel in octree structure
303 * \param[in] _point_distance distance of query point to voxel center
304 */
305 prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
306 : node(_node), point_distance(_point_distance), key(_key)
307 {}
308
309 /** \brief Operator< for comparing priority queue entries with each other.
310 * \param[in] rhs the priority queue to compare this against
311 */
312 bool
313 operator<(const prioBranchQueueEntry rhs) const
314 {
315 return (this->point_distance > rhs.point_distance);
316 }
317
318 /** \brief Pointer to octree node. */
320
321 /** \brief Distance to query point. */
323
324 /** \brief Octree key. */
326 };
327
328 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
329 /** \brief @b Priority queue entry for point candidates
330 * \note This class defines priority queue entries for the nearest neighbor point
331 * candidates.
332 * \author Julius Kammerl (julius@kammerl.de)
333 */
335 public:
336 /** \brief Empty constructor */
338
339 /** \brief Constructor for initializing priority queue entry.
340 * \param[in] point_idx index for a dataset point given by \a setInputCloud
341 * \param[in] point_distance distance of query point to voxel center
342 */
343 prioPointQueueEntry(uindex_t point_idx, float point_distance)
344 : point_idx_(point_idx), point_distance_(point_distance)
345 {}
346
347 /** \brief Operator< for comparing priority queue entries with each other.
348 * \param[in] rhs priority queue to compare this against
349 */
350 bool
351 operator<(const prioPointQueueEntry& rhs) const
352 {
353 return (this->point_distance_ < rhs.point_distance_);
354 }
355
356 /** \brief Index representing a point in the dataset given by \a setInputCloud. */
358
359 /** \brief Distance to query point. */
361 };
362
363 /** \brief Helper function to calculate the squared distance between two points
364 * \param[in] point_a point A
365 * \param[in] point_b point B
366 * \return squared distance between point A and point B
367 */
368 float
369 pointSquaredDist(const PointT& point_a, const PointT& point_b) const;
370
371 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
372 // Recursive search routine methods
373 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
374
375 /** \brief Recursive search method that explores the octree and finds neighbors within
376 * a given radius
377 * \param[in] point query point
378 * \param[in] radiusSquared squared search radius
379 * \param[in] node current octree node to be explored
380 * \param[in] key octree key addressing a leaf node.
381 * \param[in] tree_depth current depth/level in the octree
382 * \param[out] k_indices vector of indices found to be neighbors of query point
383 * \param[out] k_sqr_distances squared distances of neighbors to query point
384 * \param[in] max_nn maximum of neighbors to be found
385 */
386 void
388 const double radiusSquared,
389 const BranchNode* node,
390 const OctreeKey& key,
391 uindex_t tree_depth,
392 Indices& k_indices,
393 std::vector<float>& k_sqr_distances,
394 uindex_t max_nn) const;
395
396 /** \brief Recursive search method that explores the octree and finds the K nearest
397 * neighbors
398 * \param[in] point query point
399 * \param[in] K amount of nearest neighbors to be found
400 * \param[in] node current octree node to be explored
401 * \param[in] key octree key addressing a leaf node.
402 * \param[in] tree_depth current depth/level in the octree
403 * \param[in] squared_search_radius squared search radius distance
404 * \param[out] point_candidates priority queue of nearest neighbor point candidates
405 * \return squared search radius based on current point candidate set found
406 */
407 double
409 const PointT& point,
410 uindex_t K,
411 const BranchNode* node,
412 const OctreeKey& key,
413 uindex_t tree_depth,
414 const double squared_search_radius,
415 std::vector<prioPointQueueEntry>& point_candidates) const;
416
417 /** \brief Recursive search method that explores the octree and finds the approximate
418 * nearest neighbor
419 * \param[in] point query point
420 * \param[in] node current octree node to be explored
421 * \param[in] key octree key addressing a leaf node.
422 * \param[in] tree_depth current depth/level in the octree
423 * \param[out] result_index result index is written to this reference
424 * \param[out] sqr_distance squared distance to search
425 */
426 void
428 const BranchNode* node,
429 const OctreeKey& key,
430 uindex_t tree_depth,
431 index_t& result_index,
432 float& sqr_distance);
433
434 /** \brief Recursively search the tree for all intersected leaf nodes and return a
435 * vector of voxel centers. This algorithm is based off the paper An Efficient
436 * Parametric Algorithm for Octree Traversal:
437 * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
438 * \param[in] min_x octree nodes X coordinate of lower bounding box corner
439 * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
440 * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
441 * \param[in] max_x octree nodes X coordinate of upper bounding box corner
442 * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
443 * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
444 * \param[in] a number used for voxel child index remapping
445 * \param[in] node current octree node to be explored
446 * \param[in] key octree key addressing a leaf node.
447 * \param[out] voxel_center_list results are written to this vector of PointT elements
448 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
449 * disable)
450 * \return number of voxels found
451 */
454 double min_y,
455 double min_z,
456 double max_x,
457 double max_y,
458 double max_z,
459 unsigned char a,
460 const OctreeNode* node,
461 const OctreeKey& key,
462 AlignedPointTVector& voxel_center_list,
463 uindex_t max_voxel_count) const;
464
465 /** \brief Recursive search method that explores the octree and finds points within a
466 * rectangular search area
467 * \param[in] min_pt lower corner of search area
468 * \param[in] max_pt upper corner of search area
469 * \param[in] node current octree node to be explored
470 * \param[in] key octree key addressing a leaf node.
471 * \param[in] tree_depth current depth/level in the octree
472 * \param[out] k_indices the resultant point indices
473 */
474 void
475 boxSearchRecursive(const Eigen::Vector3f& min_pt,
476 const Eigen::Vector3f& max_pt,
477 const BranchNode* node,
478 const OctreeKey& key,
479 uindex_t tree_depth,
480 Indices& k_indices) const;
481
482 /** \brief Recursively search the tree for all intersected leaf nodes and return a
483 * vector of indices. This algorithm is based off the paper An Efficient Parametric
484 * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
485 * \param[in] min_x octree nodes X coordinate of lower bounding box corner
486 * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
487 * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
488 * \param[in] max_x octree nodes X coordinate of upper bounding box corner
489 * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
490 * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
491 * \param[in] a number used for voxel child index remapping
492 * \param[in] node current octree node to be explored
493 * \param[in] key octree key addressing a leaf node.
494 * \param[out] k_indices resulting indices
495 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
496 * disable)
497 * \return number of voxels found
498 */
501 double min_y,
502 double min_z,
503 double max_x,
504 double max_y,
505 double max_z,
506 unsigned char a,
507 const OctreeNode* node,
508 const OctreeKey& key,
509 Indices& k_indices,
510 uindex_t max_voxel_count) const;
511
512 /** \brief Initialize raytracing algorithm
513 * \param[in] origin ray origin
514 * \param[in] direction ray direction vector
515 * \param[out] min_x octree nodes X coordinate of lower bounding box corner
516 * \param[out] min_y octree nodes Y coordinate of lower bounding box corner
517 * \param[out] min_z octree nodes Z coordinate of lower bounding box corner
518 * \param[out] max_x octree nodes X coordinate of upper bounding box corner
519 * \param[out] max_y octree nodes Y coordinate of upper bounding box corner
520 * \param[out] max_z octree nodes Z coordinate of upper bounding box corner
521 * \param[out] a number used for voxel child index remapping
522 */
523 inline void
524 initIntersectedVoxel(Eigen::Vector3f& origin,
525 Eigen::Vector3f& direction,
526 double& min_x,
527 double& min_y,
528 double& min_z,
529 double& max_x,
530 double& max_y,
531 double& max_z,
532 unsigned char& a) const
533 {
534 // Account for division by zero when direction vector is 0.0
535 constexpr float epsilon = 1e-10f;
536 if (direction.x() == 0.0)
537 direction.x() = epsilon;
538 if (direction.y() == 0.0)
539 direction.y() = epsilon;
540 if (direction.z() == 0.0)
541 direction.z() = epsilon;
542
543 // Voxel childIdx remapping
544 a = 0;
545
546 // Handle negative axis direction vector
547 if (direction.x() < 0.0) {
548 origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
549 origin.x();
550 direction.x() = -direction.x();
551 a |= 4;
552 }
553 if (direction.y() < 0.0) {
554 origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
555 origin.y();
556 direction.y() = -direction.y();
557 a |= 2;
558 }
559 if (direction.z() < 0.0) {
560 origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
561 origin.z();
562 direction.z() = -direction.z();
563 a |= 1;
564 }
565 min_x = (this->min_x_ - origin.x()) / direction.x();
566 max_x = (this->max_x_ - origin.x()) / direction.x();
567 min_y = (this->min_y_ - origin.y()) / direction.y();
568 max_y = (this->max_y_ - origin.y()) / direction.y();
569 min_z = (this->min_z_ - origin.z()) / direction.z();
570 max_z = (this->max_z_ - origin.z()) / direction.z();
571 }
572
573 /** \brief Find first child node ray will enter
574 * \param[in] min_x octree nodes X coordinate of lower bounding box corner
575 * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
576 * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
577 * \param[in] mid_x octree nodes X coordinate of bounding box mid line
578 * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
579 * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
580 * \return the first child node ray will enter
581 */
582 inline int
584 double min_y,
585 double min_z,
586 double mid_x,
587 double mid_y,
588 double mid_z) const
589 {
590 int currNode = 0;
591
592 if (min_x > min_y) {
593 if (min_x > min_z) {
594 // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
595 if (mid_y < min_x)
596 currNode |= 2;
597 if (mid_z < min_x)
598 currNode |= 1;
599 }
600 else {
601 // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
602 if (mid_x < min_z)
603 currNode |= 4;
604 if (mid_y < min_z)
605 currNode |= 2;
606 }
607 }
608 else {
609 if (min_y > min_z) {
610 // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
611 if (mid_x < min_y)
612 currNode |= 4;
613 if (mid_z < min_y)
614 currNode |= 1;
615 }
616 else {
617 // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
618 if (mid_x < min_z)
619 currNode |= 4;
620 if (mid_y < min_z)
621 currNode |= 2;
622 }
623 }
624
625 return currNode;
626 }
627
628 /** \brief Get the next visited node given the current node upper
629 * bounding box corner. This function accepts three float values, and
630 * three int values. The function returns the ith integer where the
631 * ith float value is the minimum of the three float values.
632 * \param[in] x current nodes X coordinate of upper bounding box corner
633 * \param[in] y current nodes Y coordinate of upper bounding box corner
634 * \param[in] z current nodes Z coordinate of upper bounding box corner
635 * \param[in] a next node if exit Plane YZ
636 * \param[in] b next node if exit Plane XZ
637 * \param[in] c next node if exit Plane XY
638 * \return the next child node ray will enter or 8 if exiting
639 */
640 inline int
641 getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
642 {
643 if (x < y) {
644 if (x < z)
645 return a;
646 return c;
647 }
648 if (y < z)
649 return b;
650 return c;
651 }
652};
653} // namespace octree
654} // namespace pcl
655
656#ifdef PCL_NO_PRECOMPILE
657#include <pcl/octree/impl/octree_search.hpp>
658#endif
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Octree key class
Definition octree_key.h:54
Abstract octree node class
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
pcl::PointCloud< PointT > PointCloud
typename OctreeT::LeafNode LeafNode
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
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
shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
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
typename OctreeT::BranchNode BranchNode
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
typename PointCloud::Ptr PointCloudPtr
@ 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.