octree_search.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*/
#pragma once
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/point_cloud.h>
namespace pcl {
namespace octree {
/** \brief @b Octree pointcloud search class
* \note This class provides several methods for spatial neighbor search based on octree
* structure
* \tparam PointT type of point used in pointcloud
* \ingroup octree
* \author Julius Kammerl (julius@kammerl.de)
*/
template <typename PointT,
typename LeafContainerT = OctreeContainerPointIndices,
typename BranchContainerT = OctreeContainerEmpty>
class OctreePointCloudSearch
: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
public:
// public typedefs
using IndicesPtr = shared_ptr<Indices>;
using IndicesConstPtr = shared_ptr<const Indices>;
using PointCloud = pcl::PointCloud<PointT>;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
// Boost shared pointers
using Ptr =
shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
using ConstPtr = shared_ptr<
const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
// Eigen aligned allocator
using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT>>;
using OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT>;
using LeafNode = typename OctreeT::LeafNode;
using BranchNode = typename OctreeT::BranchNode;
/** \brief Constructor.
* \param[in] resolution octree resolution at lowest octree level
*/
OctreePointCloudSearch(const double resolution)
: OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution)
{}
/** \brief Search for neighbors within a voxel at given point
* \param[in] point point addressing a leaf node voxel
* \param[out] point_idx_data the resultant indices of the neighboring voxel points
* \return "true" if leaf node exist; "false" otherwise
*/
bool
voxelSearch(const PointT& point, Indices& point_idx_data);
/** \brief Search for neighbors within a voxel at given point referenced by a point
* index
* \param[in] index the index in input cloud defining the query point
* \param[out] point_idx_data the resultant indices of the neighboring voxel points
* \return "true" if leaf node exist; "false" otherwise
*/
bool
voxelSearch(uindex_t index, Indices& point_idx_data);
/** \brief Search for k-nearest neighbors at the query point.
* \param[in] cloud the point cloud data
* \param[in] index the index in \a cloud representing the query point
* \param[in] k the number of neighbors to search for
* \param[out] k_indices the resultant indices of the neighboring points (must be
* resized to \a k a priori!)
* \param[out] k_sqr_distances the resultant squared distances to the neighboring
* points (must be resized to \a k a priori!)
* \return number of neighbors found
*/
inline uindex_t
nearestKSearch(const PointCloud& cloud,
uindex_t index,
uindex_t k,
Indices& k_indices,
std::vector<float>& k_sqr_distances)
{
return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
}
/** \brief Search for k-nearest neighbors at given query point.
* \param[in] p_q the given query point
* \param[in] k the number of neighbors to search for
* \param[out] k_indices the resultant indices of the neighboring points (must be
* resized to k a priori!)
* \param[out] k_sqr_distances the resultant squared distances to the neighboring
* points (must be resized to k a priori!)
* \return number of neighbors found
*/
uindex_t
nearestKSearch(const PointT& p_q,
uindex_t k,
Indices& k_indices,
std::vector<float>& k_sqr_distances);
/** \brief Search for k-nearest neighbors at query point
* \param[in] index index representing the query point in the dataset given by \a
* setInputCloud. If indices were given in setInputCloud, index will be the position
* in the indices vector.
* \param[in] k the number of neighbors to search for
* \param[out] k_indices the resultant indices of the neighboring points (must be
* resized to \a k a priori!)
* \param[out] k_sqr_distances the resultant squared distances to the neighboring
* points (must be resized to \a k a priori!)
* \return number of neighbors found
*/
uindex_t
nearestKSearch(uindex_t index,
uindex_t k,
Indices& k_indices,
std::vector<float>& k_sqr_distances);
/** \brief Search for approx. nearest neighbor at the query point.
* \param[in] cloud the point cloud data
* \param[in] query_index the index in \a cloud representing the query point
* \param[out] result_index the resultant index of the neighbor point
* \param[out] sqr_distance the resultant squared distance to the neighboring point
* \return number of neighbors found
*/
inline void
approxNearestSearch(const PointCloud& cloud,
uindex_t query_index,
index_t& result_index,
float& sqr_distance)
{
return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
}
/** \brief Search for approx. nearest neighbor at the query point.
* \param[in] p_q the given query point
* \param[out] result_index the resultant index of the neighbor point
* \param[out] sqr_distance the resultant squared distance to the neighboring point
*/
void
approxNearestSearch(const PointT& p_q, index_t& result_index, float& sqr_distance);
/** \brief Search for approx. nearest neighbor at the query point.
* \param[in] query_index index representing the query point in the dataset given by
* \a setInputCloud. If indices were given in setInputCloud, index will be the
* position in the indices vector.
* \param[out] result_index the resultant index of the neighbor point
* \param[out] sqr_distance the resultant squared distance to the neighboring point
* \return number of neighbors found
*/
void
approxNearestSearch(uindex_t query_index, index_t& result_index, float& sqr_distance);
/** \brief Search for all neighbors of query point that are within a given radius.
* \param[in] cloud the point cloud data
* \param[in] index the index in \a cloud representing the query point
* \param[in] radius the radius of the sphere bounding all of p_q's neighbors
* \param[out] k_indices the resultant indices of the neighboring points
* \param[out] k_sqr_distances the resultant squared distances to the neighboring
* points
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value
* \return number of neighbors found in radius
*/
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)
{
return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
}
/** \brief Search for all neighbors of query point that are within a given radius.
* \param[in] p_q the given query point
* \param[in] radius the radius of the sphere bounding all of p_q's neighbors
* \param[out] k_indices the resultant indices of the neighboring points
* \param[out] k_sqr_distances the resultant squared distances to the neighboring
* points
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value
* \return number of neighbors found in radius
*/
uindex_t
radiusSearch(const PointT& p_q,
const double radius,
Indices& k_indices,
std::vector<float>& k_sqr_distances,
uindex_t max_nn = 0) const;
/** \brief Search for all neighbors of query point that are within a given radius.
* \param[in] index index representing the query point in the dataset given by \a
* setInputCloud. If indices were given in setInputCloud, index will be the position
* in the indices vector
* \param[in] radius radius of the sphere bounding all of p_q's neighbors
* \param[out] k_indices the resultant indices of the neighboring points
* \param[out] k_sqr_distances the resultant squared distances to the neighboring
* points
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value
* \return number of neighbors found in radius
*/
uindex_t
radiusSearch(uindex_t index,
const double radius,
Indices& k_indices,
std::vector<float>& k_sqr_distances,
uindex_t max_nn = 0) const;
/** \brief Get a PointT vector of centers of all voxels that intersected by a ray
* (origin, direction).
* \param[in] origin ray origin
* \param[in] direction ray direction vector
* \param[out] voxel_center_list results are written to this vector of PointT elements
* \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
* disable)
* \return number of intersected voxels
*/
uindex_t
getIntersectedVoxelCenters(Eigen::Vector3f origin,
Eigen::Vector3f direction,
AlignedPointTVector& voxel_center_list,
uindex_t max_voxel_count = 0) const;
/** \brief Get indices of all voxels that are intersected by a ray (origin,
* direction).
* \param[in] origin ray origin \param[in] direction ray direction vector
* \param[out] k_indices resulting point indices from intersected voxels
* \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
* disable)
* \return number of intersected voxels
*/
uindex_t
getIntersectedVoxelIndices(Eigen::Vector3f origin,
Eigen::Vector3f direction,
Indices& k_indices,
uindex_t max_voxel_count = 0) const;
/** \brief Search for points within rectangular search area
* Points exactly on the edges of the search rectangle are included.
* \param[in] min_pt lower corner of search area
* \param[in] max_pt upper corner of search area
* \param[out] k_indices the resultant point indices
* \return number of points found within search area
*/
uindex_t
boxSearch(const Eigen::Vector3f& min_pt,
const Eigen::Vector3f& max_pt,
Indices& k_indices) const;
protected:
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Octree-based search routines & helpers
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Priority queue entry for branch nodes
* \note This class defines priority queue entries for the nearest neighbor search.
* \author Julius Kammerl (julius@kammerl.de)
*/
class prioBranchQueueEntry {
public:
/** \brief Empty constructor */
prioBranchQueueEntry() : node(), point_distance(0) {}
/** \brief Constructor for initializing priority queue entry.
* \param _node pointer to octree node
* \param _key octree key addressing voxel in octree structure
* \param[in] _point_distance distance of query point to voxel center
*/
prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
: node(_node), point_distance(_point_distance), key(_key)
{}
/** \brief Operator< for comparing priority queue entries with each other.
* \param[in] rhs the priority queue to compare this against
*/
bool
operator<(const prioBranchQueueEntry rhs) const
{
return (this->point_distance > rhs.point_distance);
}
/** \brief Pointer to octree node. */
const OctreeNode* node;
/** \brief Distance to query point. */
float point_distance;
/** \brief Octree key. */
OctreeKey key;
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Priority queue entry for point candidates
* \note This class defines priority queue entries for the nearest neighbor point
* candidates.
* \author Julius Kammerl (julius@kammerl.de)
*/
class prioPointQueueEntry {
public:
/** \brief Empty constructor */
prioPointQueueEntry() : point_idx_(0), point_distance_(0) {}
/** \brief Constructor for initializing priority queue entry.
* \param[in] point_idx index for a dataset point given by \a setInputCloud
* \param[in] point_distance distance of query point to voxel center
*/
prioPointQueueEntry(uindex_t point_idx, float point_distance)
: point_idx_(point_idx), point_distance_(point_distance)
{}
/** \brief Operator< for comparing priority queue entries with each other.
* \param[in] rhs priority queue to compare this against
*/
bool
operator<(const prioPointQueueEntry& rhs) const
{
return (this->point_distance_ < rhs.point_distance_);
}
/** \brief Index representing a point in the dataset given by \a setInputCloud. */
uindex_t point_idx_;
/** \brief Distance to query point. */
float point_distance_;
};
/** \brief Helper function to calculate the squared distance between two points
* \param[in] point_a point A
* \param[in] point_b point B
* \return squared distance between point A and point B
*/
float
pointSquaredDist(const PointT& point_a, const PointT& point_b) const;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Recursive search routine methods
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Recursive search method that explores the octree and finds neighbors within
* a given radius
* \param[in] point query point \param[in] radiusSquared squared search radius
* \param[in] node current octree node to be explored
* \param[in] key octree key addressing a leaf node.
* \param[in] tree_depth current depth/level in the octree
* \param[out] k_indices vector of indices found to be neighbors of query point
* \param[out] k_sqr_distances squared distances of neighbors to query point
* \param[in] max_nn maximum of neighbors to be found
*/
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;
/** \brief Recursive search method that explores the octree and finds the K nearest
* neighbors
* \param[in] point query point
* \param[in] K amount of nearest neighbors to be found
* \param[in] node current octree node to be explored
* \param[in] key octree key addressing a leaf node.
* \param[in] tree_depth current depth/level in the octree
* \param[in] squared_search_radius squared search radius distance
* \param[out] point_candidates priority queue of nearest neigbor point candidates
* \return squared search radius based on current point candidate set found
*/
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;
/** \brief Recursive search method that explores the octree and finds the approximate
* nearest neighbor
* \param[in] point query point
* \param[in] node current octree node to be explored
* \param[in] key octree key addressing a leaf node.
* \param[in] tree_depth current depth/level in the octree
* \param[out] result_index result index is written to this reference
* \param[out] sqr_distance squared distance to search
*/
void
approxNearestSearchRecursive(const PointT& point,
const BranchNode* node,
const OctreeKey& key,
uindex_t tree_depth,
index_t& result_index,
float& sqr_distance);
/** \brief Recursively search the tree for all intersected leaf nodes and return a
* vector of voxel centers. This algorithm is based off the paper An Efficient
* Parametric Algorithm for Octree Traversal:
* http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
* \param[in] min_x octree nodes X coordinate of lower bounding box corner
* \param[in] min_y octree nodes Y coordinate of lower bounding box corner
* \param[in] min_z octree nodes Z coordinate of lower bounding box corner
* \param[in] max_x octree nodes X coordinate of upper bounding box corner
* \param[in] max_y octree nodes Y coordinate of upper bounding box corner
* \param[in] max_z octree nodes Z coordinate of upper bounding box corner
* \param[in] a
* \param[in] node current octree node to be explored
* \param[in] key octree key addressing a leaf node.
* \param[out] voxel_center_list results are written to this vector of PointT elements
* \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
* disable)
* \return number of voxels found
*/
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;
/** \brief Recursive search method that explores the octree and finds points within a
* rectangular search area
* \param[in] min_pt lower corner of search area
* \param[in] max_pt upper corner of search area
* \param[in] node current octree node to be explored
* \param[in] key octree key addressing a leaf node.
* \param[in] tree_depth current depth/level in the octree
* \param[out] k_indices the resultant point indices
*/
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;
/** \brief Recursively search the tree for all intersected leaf nodes and return a
* vector of indices. This algorithm is based off the paper An Efficient Parametric
* Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
* \param[in] min_x octree nodes X coordinate of lower bounding box corner
* \param[in] min_y octree nodes Y coordinate of lower bounding box corner
* \param[in] min_z octree nodes Z coordinate of lower bounding box corner
* \param[in] max_x octree nodes X coordinate of upper bounding box corner
* \param[in] max_y octree nodes Y coordinate of upper bounding box corner
* \param[in] max_z octree nodes Z coordinate of upper bounding box corner
* \param[in] a
* \param[in] node current octree node to be explored
* \param[in] key octree key addressing a leaf node.
* \param[out] k_indices resulting indices
* \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
* disable)
* \return number of voxels found
*/
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;
/** \brief Initialize raytracing algorithm
* \param origin
* \param direction
* \param[in] min_x octree nodes X coordinate of lower bounding box corner
* \param[in] min_y octree nodes Y coordinate of lower bounding box corner
* \param[in] min_z octree nodes Z coordinate of lower bounding box corner
* \param[in] max_x octree nodes X coordinate of upper bounding box corner
* \param[in] max_y octree nodes Y coordinate of upper bounding box corner
* \param[in] max_z octree nodes Z coordinate of upper bounding box corner
* \param a
*/
inline 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
{
// Account for division by zero when direction vector is 0.0
const float epsilon = 1e-10f;
if (direction.x() == 0.0)
direction.x() = epsilon;
if (direction.y() == 0.0)
direction.y() = epsilon;
if (direction.z() == 0.0)
direction.z() = epsilon;
// Voxel childIdx remapping
a = 0;
// Handle negative axis direction vector
if (direction.x() < 0.0) {
origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
origin.x();
direction.x() = -direction.x();
a |= 4;
}
if (direction.y() < 0.0) {
origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
origin.y();
direction.y() = -direction.y();
a |= 2;
}
if (direction.z() < 0.0) {
origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
origin.z();
direction.z() = -direction.z();
a |= 1;
}
min_x = (this->min_x_ - origin.x()) / direction.x();
max_x = (this->max_x_ - origin.x()) / direction.x();
min_y = (this->min_y_ - origin.y()) / direction.y();
max_y = (this->max_y_ - origin.y()) / direction.y();
min_z = (this->min_z_ - origin.z()) / direction.z();
max_z = (this->max_z_ - origin.z()) / direction.z();
}
/** \brief Find first child node ray will enter
* \param[in] min_x octree nodes X coordinate of lower bounding box corner
* \param[in] min_y octree nodes Y coordinate of lower bounding box corner
* \param[in] min_z octree nodes Z coordinate of lower bounding box corner
* \param[in] mid_x octree nodes X coordinate of bounding box mid line
* \param[in] mid_y octree nodes Y coordinate of bounding box mid line
* \param[in] mid_z octree nodes Z coordinate of bounding box mid line
* \return the first child node ray will enter
*/
inline int
getFirstIntersectedNode(double min_x,
double min_y,
double min_z,
double mid_x,
double mid_y,
double mid_z) const
{
int currNode = 0;
if (min_x > min_y) {
if (min_x > min_z) {
// max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
if (mid_y < min_x)
currNode |= 2;
if (mid_z < min_x)
currNode |= 1;
}
else {
// max(min_x, min_y, min_z) is min_z. Entry plane is XY.
if (mid_x < min_z)
currNode |= 4;
if (mid_y < min_z)
currNode |= 2;
}
}
else {
if (min_y > min_z) {
// max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
if (mid_x < min_y)
currNode |= 4;
if (mid_z < min_y)
currNode |= 1;
}
else {
// max(min_x, min_y, min_z) is min_z. Entry plane is XY.
if (mid_x < min_z)
currNode |= 4;
if (mid_y < min_z)
currNode |= 2;
}
}
return currNode;
}
/** \brief Get the next visited node given the current node upper
* bounding box corner. This function accepts three float values, and
* three int values. The function returns the ith integer where the
* ith float value is the minimum of the three float values.
* \param[in] x current nodes X coordinate of upper bounding box corner
* \param[in] y current nodes Y coordinate of upper bounding box corner
* \param[in] z current nodes Z coordinate of upper bounding box corner
* \param[in] a next node if exit Plane YZ
* \param[in] b next node if exit Plane XZ
* \param[in] c next node if exit Plane XY
* \return the next child node ray will enter or 8 if exiting
*/
inline int
getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
{
if (x < y) {
if (x < z)
return a;
return c;
}
if (y < z)
return b;
return c;
}
};
} // namespace octree
} // namespace pcl
#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree_search.hpp>
#endif
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:52
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:58
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:72
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_y_
double min_y_
Definition: octree_pointcloud.h:587
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_z_
double max_z_
Definition: octree_pointcloud.h:591
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_y_
double max_y_
Definition: octree_pointcloud.h:588
pcl::octree::OctreePointCloud::LeafNode
typename OctreeT::LeafNode LeafNode
Definition: octree_pointcloud.h:76
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_x_
double min_x_
Definition: octree_pointcloud.h:584
pcl::octree::OctreePointCloud::BranchNode
typename OctreeT::BranchNode BranchNode
Definition: octree_pointcloud.h:77
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_z_
double min_z_
Definition: octree_pointcloud.h:590
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_x_
double max_x_
Definition: octree_pointcloud.h:585
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry
Priority queue entry for branch nodes
Definition: octree_search.h:296
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::operator<
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:314
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::key
OctreeKey key
Octree key.
Definition: octree_search.h:326
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::node
const OctreeNode * node
Pointer to octree node.
Definition: octree_search.h:320
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:306
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry()
Empty constructor
Definition: octree_search.h:299
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::point_distance
float point_distance
Distance to query point.
Definition: octree_search.h:323
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry
Priority queue entry for point candidates
Definition: octree_search.h:335
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry(uindex_t point_idx, float point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:344
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry()
Empty constructor
Definition: octree_search.h:338
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_idx_
uindex_t point_idx_
Index representing a point in the dataset given by setInputCloud.
Definition: octree_search.h:358
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::operator<
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:352
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_distance_
float point_distance_
Distance to query point.
Definition: octree_search.h:361
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:58
pcl::octree::OctreePointCloudSearch::LeafNode
typename OctreeT::LeafNode LeafNode
Definition: octree_search.h:78
pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive
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.
Definition: octree_search.hpp:224
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices
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).
Definition: octree_search.hpp:631
pcl::octree::OctreePointCloudSearch::getNextIntersectedNode
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.
Definition: octree_search.h:641
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndicesRecursive
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.
Definition: octree_search.hpp:861
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters
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).
Definition: octree_search.hpp:595
pcl::octree::OctreePointCloudSearch::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_search.h:66
pcl::octree::OctreePointCloudSearch::voxelSearch
bool voxelSearch(const PointT &point, Indices &point_idx_data)
Search for neighbors within a voxel at given point.
Definition: octree_search.hpp:50
pcl::octree::OctreePointCloudSearch::ConstPtr
shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:72
pcl::octree::OctreePointCloudSearch::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: octree_search.h:61
pcl::octree::OctreePointCloudSearch::Ptr
shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:70
pcl::octree::OctreePointCloudSearch::boxSearch
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 ...
Definition: octree_search.hpp:205
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCentersRecursive
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.
Definition: octree_search.hpp:665
pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive
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.
Definition: octree_search.hpp:423
pcl::octree::OctreePointCloudSearch::nearestKSearch
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.
Definition: octree_search.h:116
pcl::octree::OctreePointCloudSearch::boxSearchRecursive
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.
Definition: octree_search.hpp:519
pcl::octree::OctreePointCloudSearch::initIntersectedVoxel
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.
Definition: octree_search.h:524
pcl::octree::OctreePointCloudSearch::pointSquaredDist
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
Definition: octree_search.hpp:511
pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive
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.
Definition: octree_search.hpp:336
pcl::octree::OctreePointCloudSearch::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: octree_search.h:62
pcl::octree::OctreePointCloudSearch::BranchNode
typename OctreeT::BranchNode BranchNode
Definition: octree_search.h:79
pcl::octree::OctreePointCloudSearch::approxNearestSearch
void approxNearestSearch(const PointCloud &cloud, uindex_t query_index, index_t &result_index, float &sqr_distance)
Search for approx.
Definition: octree_search.h:165
pcl::octree::OctreePointCloudSearch::getFirstIntersectedNode
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.
Definition: octree_search.h:583
pcl::octree::OctreePointCloudSearch::radiusSearch
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.
Definition: octree_search.h:203
pcl::octree::OctreePointCloudSearch::OctreePointCloudSearch
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:84
pcl::octree::OctreePointCloudSearch::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:75
pcl::octree::OctreePointCloudSearch::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_search.h:65
pcl::K
@ K
Definition: norms.h:54
pcl
Definition: convolution.h:46
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
pcl::index_t
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
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:675