Point Cloud Library (PCL) 1.12.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 * \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 \param[in] direction ray direction vector
265 * \param[out] k_indices resulting point indices from intersected voxels
266 * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
267 * disable)
268 * \return number of intersected voxels
269 */
271 getIntersectedVoxelIndices(Eigen::Vector3f origin,
272 Eigen::Vector3f direction,
273 Indices& k_indices,
274 uindex_t max_voxel_count = 0) const;
275
276 /** \brief Search for points within rectangular search area
277 * Points exactly on the edges of the search rectangle are included.
278 * \param[in] min_pt lower corner of search area
279 * \param[in] max_pt upper corner of search area
280 * \param[out] k_indices the resultant point indices
281 * \return number of points found within search area
282 */
284 boxSearch(const Eigen::Vector3f& min_pt,
285 const Eigen::Vector3f& max_pt,
286 Indices& k_indices) const;
287
288protected:
289 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
290 // Octree-based search routines & helpers
291 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292 /** \brief @b Priority queue entry for branch nodes
293 * \note This class defines priority queue entries for the nearest neighbor search.
294 * \author Julius Kammerl (julius@kammerl.de)
295 */
297 public:
298 /** \brief Empty constructor */
300
301 /** \brief Constructor for initializing priority queue entry.
302 * \param _node pointer to octree node
303 * \param _key octree key addressing voxel in octree structure
304 * \param[in] _point_distance distance of query point to voxel center
305 */
306 prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
307 : node(_node), point_distance(_point_distance), key(_key)
308 {}
309
310 /** \brief Operator< for comparing priority queue entries with each other.
311 * \param[in] rhs the priority queue to compare this against
312 */
313 bool
314 operator<(const prioBranchQueueEntry rhs) const
315 {
316 return (this->point_distance > rhs.point_distance);
317 }
318
319 /** \brief Pointer to octree node. */
321
322 /** \brief Distance to query point. */
324
325 /** \brief Octree key. */
327 };
328
329 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
330 /** \brief @b Priority queue entry for point candidates
331 * \note This class defines priority queue entries for the nearest neighbor point
332 * candidates.
333 * \author Julius Kammerl (julius@kammerl.de)
334 */
336 public:
337 /** \brief Empty constructor */
339
340 /** \brief Constructor for initializing priority queue entry.
341 * \param[in] point_idx index for a dataset point given by \a setInputCloud
342 * \param[in] point_distance distance of query point to voxel center
343 */
344 prioPointQueueEntry(uindex_t point_idx, float point_distance)
345 : point_idx_(point_idx), point_distance_(point_distance)
346 {}
347
348 /** \brief Operator< for comparing priority queue entries with each other.
349 * \param[in] rhs priority queue to compare this against
350 */
351 bool
352 operator<(const prioPointQueueEntry& rhs) const
353 {
354 return (this->point_distance_ < rhs.point_distance_);
355 }
356
357 /** \brief Index representing a point in the dataset given by \a setInputCloud. */
359
360 /** \brief Distance to query point. */
362 };
363
364 /** \brief Helper function to calculate the squared distance between two points
365 * \param[in] point_a point A
366 * \param[in] point_b point B
367 * \return squared distance between point A and point B
368 */
369 float
370 pointSquaredDist(const PointT& point_a, const PointT& point_b) const;
371
372 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
373 // Recursive search routine methods
374 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
375
376 /** \brief Recursive search method that explores the octree and finds neighbors within
377 * a given radius
378 * \param[in] point query point \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 neigbor 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
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
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 origin
514 * \param direction
515 * \param[in] min_x octree nodes X coordinate of lower bounding box corner
516 * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
517 * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
518 * \param[in] max_x octree nodes X coordinate of upper bounding box corner
519 * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
520 * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
521 * \param a
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 const 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:52
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
shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
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
shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
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.
typename PointCloud::Ptr PointCloudPtr
@ K
Definition norms.h:54
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
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
A point structure representing Euclidean xyz coordinates, and the RGB color.