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$
*/
#ifndef PCL_OCTREE_SEARCH_H_
#define PCL_OCTREE_SEARCH_H_
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "octree_pointcloud.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
* \note typename: 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
typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
typedef pcl::PointCloud<PointT> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
// Boost shared pointers
typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > Ptr;
typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > ConstPtr;
// Eigen aligned allocator
typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT> OctreeT;
typedef typename OctreeT::LeafNode LeafNode;
typedef typename OctreeT::BranchNode BranchNode;
/** \brief Constructor.
* \param[in] resolution octree resolution at lowest octree level
*/
OctreePointCloudSearch (const double resolution) :
OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution)
{
}
/** \brief Empty class constructor. */
virtual
~OctreePointCloudSearch ()
{
}
/** \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, std::vector<int>& 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 (const int index, std::vector<int>& 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 int
nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &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
*/
int
nearestKSearch (const PointT &p_q, int k, std::vector<int> &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
*/
int
nearestKSearch (int index, int k, std::vector<int> &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, int query_index, int &result_index, float &sqr_distance)
{
return (approxNearestSearch (cloud.points[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, int &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 (int query_index, int &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
*/
int
radiusSearch (const PointCloud &cloud, int index, double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
{
return (radiusSearch (cloud.points[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
*/
int
radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int 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
*/
int
radiusSearch (int index, const double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int 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
*/
int
getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
AlignedPointTVector &voxel_center_list, int 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
*/
int
getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
std::vector<int> &k_indices,
int max_voxel_count = 0) const;
/** \brief Search for points within rectangular search area
* \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
*/
int
boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &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), key ()
{
}
/** \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 an index representing a point in the dataset given by \a setInputCloud
* \param[in] point_distance distance of query point to voxel center
*/
prioPointQueueEntry (unsigned int& 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. */
int 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,
unsigned int tree_depth, std::vector<int>& k_indices,
std::vector<float>& k_sqr_distances, unsigned int 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, unsigned int K, const BranchNode* node,
const OctreeKey& key, unsigned int 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,
unsigned int tree_depth, int& 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
*/
int
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,
int 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, unsigned int tree_depth, std::vector<int>& 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
*/
int
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,
std::vector<int> &k_indices,
int 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;
else
return c;
}
else
{
if (y < z)
return b;
else
return c;
}
return 0;
}
};
}
}
#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree_search.hpp>
#else
#define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
#endif // PCL_NO_PRECOMPILE
#endif // PCL_OCTREE_SEARCH_H_
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndicesRecursive
int 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, std::vector< int > &k_indices, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices...
Definition: octree_search.hpp:754
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::min_z_
double min_z_
Definition: octree_pointcloud.h:561
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:77
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:276
pcl::octree::OctreePointCloudSearch::voxelSearch
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
Definition: octree_search.hpp:51
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry
Priority queue entry for branch nodes
Definition: octree_search.h:262
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:520
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::octree::OctreePointCloudSearch::PointCloudConstPtr
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: octree_search.h:67
pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius...
Definition: octree_search.hpp:322
pcl::octree::OctreePointCloudSearch::IndicesPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_search.h:62
pcl::octree::OctreePointCloudSearch::BranchNode
OctreeT::BranchNode BranchNode
Definition: octree_search.h:78
pcl::octree::OctreePointCloudSearch::OctreeT
OctreePointCloud< PointT, LeafContainerT, BranchContainerT > OctreeT
Definition: octree_search.h:76
pcl
Definition: cloud_iterator.h:46
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:579
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCentersRecursive
int 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, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers...
Definition: octree_search.hpp:636
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry
Priority queue entry for point candidates
Definition: octree_search.h:305
pcl::octree::OctreePointCloudSearch::nearestKSearch
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
Definition: octree_search.h:120
pcl::octree::OctreePointCloudSearch::boxSearchRecursive
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area...
Definition: octree_search.hpp:511
pcl::octree::OctreePointCloudSearch::approxNearestSearch
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
Definition: octree_search.h:157
pcl::octree::OctreePointCloudSearch::IndicesConstPtr
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_search.h:63
pcl::octree::OctreePointCloudSearch::~OctreePointCloudSearch
virtual ~OctreePointCloudSearch()
Empty class constructor.
Definition: octree_search.h:90
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:319
pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive
double getKNearestNeighborRecursive(const PointT &point, unsigned int K, const BranchNode *node, const OctreeKey &key, unsigned int 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:213
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::max_y_
double max_y_
Definition: octree_pointcloud.h:559
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:503
pcl::octree::OctreePointCloudSearch::Ptr
boost::shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:70
pcl::octree::OctreePointCloudSearch::boxSearch
int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
Search for points within rectangular search area.
Definition: octree_search.hpp:195
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_idx_
int point_idx_
Index representing a point in the dataset given by setInputCloud.
Definition: octree_search.h:334
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::node
const OctreeNode * node
Pointer to octree node.
Definition: octree_search.h:291
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::key
OctreeKey key
Octree key.
Definition: octree_search.h:297
pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, int &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor...
Definition: octree_search.hpp:407
pcl::octree::OctreePointCloudSearch::PointCloudPtr
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: octree_search.h:66
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters
int getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, int 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:588
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry()
Empty constructor.
Definition: octree_search.h:310
pcl::octree::OctreePointCloudSearch::LeafNode
OctreeT::LeafNode LeafNode
Definition: octree_search.h:77
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices
int getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
Definition: octree_search.hpp:613
pcl::octree::OctreePointCloudSearch::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:74
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
pcl::octree::OctreePointCloudSearch::PointCloud
pcl::PointCloud< PointT > PointCloud
Definition: octree_search.h:65
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:53
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::point_distance
float point_distance
Distance to query point.
Definition: octree_search.h:294
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:100
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::operator<
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:285
pcl::K
Definition: norms.h:55
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:58
pcl::octree::OctreePointCloudSearch::radiusSearch
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all neighbors of query point that are within a given radius.
Definition: octree_search.h:190
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::min_y_
double min_y_
Definition: octree_pointcloud.h:558
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::max_x_
double max_x_
Definition: octree_pointcloud.h:556
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::min_x_
double min_x_
Definition: octree_pointcloud.h:555
pcl::octree::OctreePointCloudSearch::OctreePointCloudSearch
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:83
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:594
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:207
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_distance_
float point_distance_
Distance to query point.
Definition: octree_search.h:337
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::max_z_
double max_z_
Definition: octree_pointcloud.h:562
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:71
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry()
Empty constructor.
Definition: octree_search.h:266
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:466
pcl::octree::OctreePointCloudSearch::ConstPtr
boost::shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:71