Point Cloud Library (PCL)
1.14.1-dev
|
Octree pointcloud search class More...
#include <pcl/octree/octree_search.h>
Classes | |
class | prioBranchQueueEntry |
Priority queue entry for branch nodes More... | |
class | prioPointQueueEntry |
Priority queue entry for point candidates More... | |
Public Member Functions | |
OctreePointCloudSearch (const double resolution) | |
Constructor. More... | |
bool | voxelSearch (const PointT &point, Indices &point_idx_data) |
Search for neighbors within a voxel at given point. More... | |
bool | voxelSearch (uindex_t index, Indices &point_idx_data) |
Search for neighbors within a voxel at given point referenced by a point index. More... | |
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. More... | |
uindex_t | nearestKSearch (const PointT &p_q, uindex_t k, Indices &k_indices, std::vector< float > &k_sqr_distances) |
Search for k-nearest neighbors at given query point. More... | |
uindex_t | nearestKSearch (uindex_t index, uindex_t k, Indices &k_indices, std::vector< float > &k_sqr_distances) |
Search for k-nearest neighbors at query point. More... | |
void | approxNearestSearch (const PointCloud &cloud, uindex_t query_index, index_t &result_index, float &sqr_distance) |
Search for approx. More... | |
void | approxNearestSearch (const PointT &p_q, index_t &result_index, float &sqr_distance) |
Search for approx. More... | |
void | approxNearestSearch (uindex_t query_index, index_t &result_index, float &sqr_distance) |
Search for approx. More... | |
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. More... | |
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 |
Search for all neighbors of query point that are within a given radius. More... | |
uindex_t | radiusSearch (uindex_t index, const double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, uindex_t max_nn=0) const |
Search for all neighbors of query point that are within a given radius. More... | |
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). More... | |
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). More... | |
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 are included. More... | |
Public Member Functions inherited from pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty > | |
OctreePointCloud (const double resolution_arg) | |
Octree pointcloud constructor. More... | |
void | setInputCloud (const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr()) |
Provide a pointer to the input data set. More... | |
IndicesConstPtr const | getIndices () const |
Get a pointer to the vector of indices used. More... | |
PointCloudConstPtr | getInputCloud () const |
Get a pointer to the input point cloud dataset. More... | |
void | setEpsilon (double eps) |
Set the search epsilon precision (error bound) for nearest neighbors searches. More... | |
double | getEpsilon () const |
Get the search epsilon precision (error bound) for nearest neighbors searches. More... | |
void | setResolution (double resolution_arg) |
Set/change the octree voxel resolution. More... | |
double | getResolution () const |
Get octree voxel resolution. More... | |
uindex_t | getTreeDepth () const |
Get the maximum depth of the octree. More... | |
void | addPointsFromInputCloud () |
Add points from input point cloud to octree. More... | |
void | addPointFromCloud (uindex_t point_idx_arg, IndicesPtr indices_arg) |
Add point at given index from input point cloud to octree. More... | |
void | addPointToCloud (const PointT &point_arg, PointCloudPtr cloud_arg) |
Add point simultaneously to octree and input point cloud. More... | |
void | addPointToCloud (const PointT &point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg) |
Add point simultaneously to octree and input point cloud. More... | |
bool | isVoxelOccupiedAtPoint (const PointT &point_arg) const |
Check if voxel at given point exist. More... | |
bool | isVoxelOccupiedAtPoint (const double point_x_arg, const double point_y_arg, const double point_z_arg) const |
Check if voxel at given point coordinates exist. More... | |
bool | isVoxelOccupiedAtPoint (const index_t &point_idx_arg) const |
Check if voxel at given point from input cloud exist. More... | |
void | deleteTree () |
Delete the octree structure and its leaf nodes. More... | |
uindex_t | getOccupiedVoxelCenters (AlignedPointTVector &voxel_center_list_arg) const |
Get a PointT vector of centers of all occupied voxels. More... | |
uindex_t | getApproxIntersectedVoxelCentersBySegment (const Eigen::Vector3f &origin, const Eigen::Vector3f &end, AlignedPointTVector &voxel_center_list, float precision=0.2) |
Get a PointT vector of centers of voxels intersected by a line segment. More... | |
void | deleteVoxelAtPoint (const PointT &point_arg) |
Delete leaf node / voxel at given point. More... | |
void | deleteVoxelAtPoint (const index_t &point_idx_arg) |
Delete leaf node / voxel at given point from input cloud. More... | |
void | defineBoundingBox () |
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree. More... | |
void | defineBoundingBox (const double min_x_arg, const double min_y_arg, const double min_z_arg, const double max_x_arg, const double max_y_arg, const double max_z_arg) |
Define bounding box for octree. More... | |
void | defineBoundingBox (const double max_x_arg, const double max_y_arg, const double max_z_arg) |
Define bounding box for octree. More... | |
void | defineBoundingBox (const double cubeLen_arg) |
Define bounding box cube for octree. More... | |
void | getBoundingBox (double &min_x_arg, double &min_y_arg, double &min_z_arg, double &max_x_arg, double &max_y_arg, double &max_z_arg) const |
Get bounding box for octree. More... | |
double | getVoxelSquaredDiameter (uindex_t tree_depth_arg) const |
Calculates the squared diameter of a voxel at given tree depth. More... | |
double | getVoxelSquaredDiameter () const |
Calculates the squared diameter of a voxel at leaf depth. More... | |
double | getVoxelSquaredSideLen (uindex_t tree_depth_arg) const |
Calculates the squared voxel cube side length at given tree depth. More... | |
double | getVoxelSquaredSideLen () const |
Calculates the squared voxel cube side length at leaf level. More... | |
void | getVoxelBounds (const OctreeIteratorBase< OctreeBase< OctreeContainerPointIndices, OctreeContainerEmpty > > &iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const |
Generate bounds of the current voxel of an octree iterator. More... | |
void | enableDynamicDepth (std::size_t maxObjsPerLeaf) |
Enable dynamic octree structure. More... | |
Public Member Functions inherited from pcl::octree::OctreeBase< OctreeContainerPointIndices, OctreeContainerEmpty > | |
Iterator | begin (uindex_t max_depth_arg=0u) |
ConstIterator | begin (uindex_t max_depth_arg=0u) const |
ConstIterator | cbegin (uindex_t max_depth_arg=0u) const |
const Iterator | end () |
const ConstIterator | end () const |
const ConstIterator | cend () const |
LeafNodeDepthFirstIterator | leaf_depth_begin (uindex_t max_depth_arg=0u) |
ConstLeafNodeDepthFirstIterator | leaf_depth_begin (uindex_t max_depth_arg=0u) const |
const LeafNodeDepthFirstIterator | leaf_depth_end () |
const ConstLeafNodeDepthFirstIterator | leaf_depth_end () const |
DepthFirstIterator | depth_begin (uindex_t max_depth_arg=0u) |
ConstDepthFirstIterator | depth_begin (uindex_t max_depth_arg=0u) const |
const DepthFirstIterator | depth_end () |
const ConstDepthFirstIterator | depth_end () const |
BreadthFirstIterator | breadth_begin (uindex_t max_depth_arg=0u) |
ConstBreadthFirstIterator | breadth_begin (uindex_t max_depth_arg=0u) const |
const BreadthFirstIterator | breadth_end () |
const ConstBreadthFirstIterator | breadth_end () const |
FixedDepthIterator | fixed_depth_begin (uindex_t fixed_depth_arg=0u) |
ConstFixedDepthIterator | fixed_depth_begin (uindex_t fixed_depth_arg=0u) const |
const FixedDepthIterator | fixed_depth_end () |
const ConstFixedDepthIterator | fixed_depth_end () const |
LeafNodeBreadthFirstIterator | leaf_breadth_begin (uindex_t max_depth_arg=0u) |
ConstLeafNodeBreadthFirstIterator | leaf_breadth_begin (uindex_t max_depth_arg=0u) const |
const LeafNodeBreadthFirstIterator | leaf_breadth_end () |
const ConstLeafNodeBreadthFirstIterator | leaf_breadth_end () const |
OctreeBase () | |
Empty constructor. More... | |
OctreeBase (const OctreeBase &source) | |
Copy constructor. More... | |
virtual | ~OctreeBase () |
Empty deconstructor. More... | |
OctreeBase & | operator= (const OctreeBase &source) |
Copy operator. More... | |
void | setMaxVoxelIndex (uindex_t max_voxel_index_arg) |
Set the maximum amount of voxels per dimension. More... | |
void | setTreeDepth (uindex_t max_depth_arg) |
Set the maximum depth of the octree. More... | |
uindex_t | getTreeDepth () const |
Get the maximum depth of the octree. More... | |
OctreeContainerPointIndices * | createLeaf (uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) |
Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). More... | |
OctreeContainerPointIndices * | findLeaf (uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const |
Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). More... | |
bool | existLeaf (uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const |
idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). More... | |
void | removeLeaf (uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) |
Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). More... | |
std::size_t | getLeafCount () const |
Return the amount of existing leafs in the octree. More... | |
std::size_t | getBranchCount () const |
Return the amount of existing branch nodes in the octree. More... | |
void | deleteTree () |
Delete the octree structure and its leaf nodes. More... | |
void | serializeTree (std::vector< char > &binary_tree_out_arg) const |
Serialize octree into a binary output vector describing its branch node structure. More... | |
void | serializeTree (std::vector< char > &binary_tree_out_arg, std::vector< OctreeContainerPointIndices * > &leaf_container_vector_arg) const |
Serialize octree into a binary output vector describing its branch node structure and push all LeafContainerT elements stored in the octree to a vector. More... | |
void | serializeLeafs (std::vector< OctreeContainerPointIndices * > &leaf_container_vector_arg) |
Outputs a vector of all LeafContainerT elements that are stored within the octree leaf nodes. More... | |
void | deserializeTree (std::vector< char > &binary_tree_input_arg) |
Deserialize a binary octree description vector and create a corresponding octree structure. More... | |
void | deserializeTree (std::vector< char > &binary_tree_input_arg, std::vector< OctreeContainerPointIndices * > &leaf_container_vector_arg) |
Deserialize a binary octree description and create a corresponding octree structure. More... | |
Protected Member Functions | |
float | pointSquaredDist (const PointT &point_a, const PointT &point_b) const |
Helper function to calculate the squared distance between two points. More... | |
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. More... | |
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. More... | |
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. More... | |
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. More... | |
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. More... | |
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. More... | |
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. More... | |
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. More... | |
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. More... | |
Protected Member Functions inherited from pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty > | |
virtual void | addPointIdx (uindex_t point_idx_arg) |
Add point at index from input pointcloud dataset to octree. More... | |
void | expandLeafNode (LeafNode *leaf_node, BranchNode *parent_branch, unsigned char child_idx, uindex_t depth_mask) |
Add point at index from input pointcloud dataset to octree. More... | |
const PointT & | getPointByIndex (uindex_t index_arg) const |
Get point at index from input pointcloud dataset. More... | |
OctreeContainerPointIndices * | findLeafAtPoint (const PointT &point_arg) const |
Find octree leaf node at a given point. More... | |
void | getKeyBitSize () |
Define octree key setting and octree depth based on defined bounding box. More... | |
void | adoptBoundingBoxToPoint (const PointT &point_idx_arg) |
Grow the bounding box/octree until point fits. More... | |
bool | isPointWithinBoundingBox (const PointT &point_idx_arg) const |
Checks if given point is within the bounding box of the octree. More... | |
void | genOctreeKeyforPoint (const PointT &point_arg, OctreeKey &key_arg) const |
Generate octree key for voxel at a given point. More... | |
void | genOctreeKeyforPoint (const double point_x_arg, const double point_y_arg, const double point_z_arg, OctreeKey &key_arg) const |
Generate octree key for voxel at a given point. More... | |
virtual bool | genOctreeKeyForDataT (const index_t &data_arg, OctreeKey &key_arg) const |
Virtual method for generating octree key for a given point index. More... | |
void | genLeafNodeCenterFromOctreeKey (const OctreeKey &key_arg, PointT &point_arg) const |
Generate a point at center of leaf node voxel. More... | |
void | genVoxelCenterFromOctreeKey (const OctreeKey &key_arg, uindex_t tree_depth_arg, PointT &point_arg) const |
Generate a point at center of octree voxel at given tree level. More... | |
void | genVoxelBoundsFromOctreeKey (const OctreeKey &key_arg, uindex_t tree_depth_arg, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const |
Generate bounds of an octree voxel using octree key and tree depth arguments. More... | |
uindex_t | getOccupiedVoxelCentersRecursive (const BranchNode *node_arg, const OctreeKey &key_arg, AlignedPointTVector &voxel_center_list_arg) const |
Recursively search the tree for all leaf nodes and return a vector of voxel centers. More... | |
Protected Member Functions inherited from pcl::octree::OctreeBase< OctreeContainerPointIndices, OctreeContainerEmpty > | |
OctreeContainerPointIndices * | createLeaf (const OctreeKey &key_arg) |
Create a leaf node. More... | |
OctreeContainerPointIndices * | findLeaf (const OctreeKey &key_arg) const |
Find leaf node. More... | |
bool | existLeaf (const OctreeKey &key_arg) const |
Check for existence of a leaf node in the octree. More... | |
void | removeLeaf (const OctreeKey &key_arg) |
Remove leaf node from octree. More... | |
OctreeNode * | getRootNode () const |
Retrieve root node. More... | |
bool | branchHasChild (const BranchNode &branch_arg, unsigned char child_idx_arg) const |
Check if branch is pointing to a particular child node. More... | |
OctreeNode * | getBranchChildPtr (const BranchNode &branch_arg, unsigned char child_idx_arg) const |
Retrieve a child node pointer for child node at child_idx. More... | |
void | setBranchChildPtr (BranchNode &branch_arg, unsigned char child_idx_arg, OctreeNode *new_child_arg) |
Assign new child node to branch. More... | |
char | getBranchBitPattern (const BranchNode &branch_arg) const |
Generate bit pattern reflecting the existence of child node pointers. More... | |
void | deleteBranchChild (BranchNode &branch_arg, unsigned char child_idx_arg) |
Delete child node and all its subchilds from octree. More... | |
void | deleteBranch (BranchNode &branch_arg) |
Delete branch and all its subchilds from octree. More... | |
BranchNode * | createBranchChild (BranchNode &branch_arg, unsigned char child_idx_arg) |
Create and add a new branch child to a branch class. More... | |
LeafNode * | createLeafChild (BranchNode &branch_arg, unsigned char child_idx_arg) |
Create and add a new leaf child to a branch class. More... | |
uindex_t | createLeafRecursive (const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg, LeafNode *&return_leaf_arg, BranchNode *&parent_of_leaf_arg) |
Create a leaf node at octree key. More... | |
void | findLeafRecursive (const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg, OctreeContainerPointIndices *&result_arg) const |
Recursively search for a given leaf node and return a pointer. More... | |
bool | deleteLeafRecursive (const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg) |
Recursively search and delete leaf node. More... | |
void | serializeTreeRecursive (const BranchNode *branch_arg, OctreeKey &key_arg, std::vector< char > *binary_tree_out_arg, typename std::vector< OctreeContainerPointIndices * > *leaf_container_vector_arg) const |
Recursively explore the octree and output binary octree description together with a vector of leaf node LeafContainerTs. More... | |
void | deserializeTreeRecursive (BranchNode *branch_arg, uindex_t depth_mask_arg, OctreeKey &key_arg, typename std::vector< char >::const_iterator &binary_tree_input_it_arg, typename std::vector< char >::const_iterator &binary_tree_input_it_end_arg, typename std::vector< OctreeContainerPointIndices * >::const_iterator *leaf_container_vector_it_arg, typename std::vector< OctreeContainerPointIndices * >::const_iterator *leaf_container_vector_it_end_arg) |
Recursive method for deserializing octree structure. More... | |
virtual void | serializeTreeCallback (OctreeContainerPointIndices &, const OctreeKey &) const |
Callback executed for every leaf node during serialization. More... | |
virtual void | deserializeTreeCallback (OctreeContainerPointIndices &, const OctreeKey &) |
Callback executed for every leaf node during deserialization. More... | |
bool | octreeCanResize () const |
Test if octree is able to dynamically change its depth. More... | |
Additional Inherited Members | |
Protected Attributes inherited from pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty > | |
PointCloudConstPtr | input_ |
Pointer to input point cloud dataset. More... | |
IndicesConstPtr | indices_ |
A pointer to the vector of point indices to use. More... | |
double | epsilon_ |
Epsilon precision (error bound) for nearest neighbors searches. More... | |
double | resolution_ |
Octree resolution. More... | |
double | min_x_ |
double | max_x_ |
double | min_y_ |
double | max_y_ |
double | min_z_ |
double | max_z_ |
bool | bounding_box_defined_ |
Flag indicating if octree has defined bounding box. More... | |
std::size_t | max_objs_per_leaf_ |
Amount of DataT objects per leafNode before expanding branch. More... | |
Protected Attributes inherited from pcl::octree::OctreeBase< OctreeContainerPointIndices, OctreeContainerEmpty > | |
std::size_t | leaf_count_ |
Amount of leaf nodes More... | |
std::size_t | branch_count_ |
Amount of branch nodes More... | |
BranchNode * | root_node_ |
Pointer to root branch node of octree More... | |
uindex_t | depth_mask_ |
Depth mask based on octree depth More... | |
uindex_t | octree_depth_ |
Octree depth. More... | |
bool | dynamic_depth_enabled_ |
Enable dynamic_depth. More... | |
OctreeKey | max_key_ |
key range More... | |
Octree pointcloud search class
PointT | type of point used in pointcloud |
Definition at line 57 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT> > |
Definition at line 75 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::BranchNode = typename OctreeT::BranchNode |
Definition at line 79 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::ConstPtr = shared_ptr< const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > |
Definition at line 71 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::IndicesConstPtr = shared_ptr<const Indices> |
Definition at line 62 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::IndicesPtr = shared_ptr<Indices> |
Definition at line 61 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::LeafNode = typename OctreeT::LeafNode |
Definition at line 78 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT> |
Definition at line 77 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::PointCloud = pcl::PointCloud<PointT> |
Definition at line 64 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::PointCloudConstPtr = typename PointCloud::ConstPtr |
Definition at line 66 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::PointCloudPtr = typename PointCloud::Ptr |
Definition at line 65 of file octree_search.h.
using pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::Ptr = shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > |
Definition at line 69 of file octree_search.h.
|
inline |
Constructor.
[in] | resolution | octree resolution at lowest octree level |
Definition at line 84 of file octree_search.h.
|
inline |
Search for approx.
nearest neighbor at the query point.
[in] | cloud | the point cloud data |
[in] | query_index | the index in cloud representing the query point |
[out] | result_index | the resultant index of the neighbor point |
[out] | sqr_distance | the resultant squared distance to the neighboring point |
Definition at line 164 of file octree_search.h.
void pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::approxNearestSearch | ( | const PointT & | p_q, |
index_t & | result_index, | ||
float & | sqr_distance | ||
) |
Search for approx.
nearest neighbor at the query point.
[in] | p_q | the given query point |
[out] | result_index | the resultant index of the neighbor point |
[out] | sqr_distance | the resultant squared distance to the neighboring point |
Definition at line 135 of file octree_search.hpp.
References pcl::isFinite(), pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
void pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::approxNearestSearch | ( | uindex_t | query_index, |
index_t & | result_index, | ||
float & | sqr_distance | ||
) |
Search for approx.
nearest neighbor at the query point.
[in] | query_index | index representing the query point in the dataset given by setInputCloud. If indices were given in setInputCloud, index will be the position in the indices vector. |
[out] | result_index | the resultant index of the neighbor point |
[out] | sqr_distance | the resultant squared distance to the neighboring point |
Definition at line 153 of file octree_search.hpp.
|
protected |
Recursive search method that explores the octree and finds the approximate nearest neighbor.
[in] | point | query point |
[in] | node | current octree node to be explored |
[in] | key | octree key addressing a leaf node. |
[in] | tree_depth | current depth/level in the octree |
[out] | result_index | result index is written to this reference |
[out] | sqr_distance | squared distance to search |
Definition at line 431 of file octree_search.hpp.
uindex_t pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::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 are included.
[in] | min_pt | lower corner of search area |
[in] | max_pt | upper corner of search area |
[out] | k_indices | the resultant point indices |
Definition at line 206 of file octree_search.hpp.
References pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
Referenced by pcl::applyMorphologicalOperator().
|
protected |
Recursive search method that explores the octree and finds points within a rectangular search area.
[in] | min_pt | lower corner of search area |
[in] | max_pt | upper corner of search area |
[in] | node | current octree node to be explored |
[in] | key | octree key addressing a leaf node. |
[in] | tree_depth | current depth/level in the octree |
[out] | k_indices | the resultant point indices |
Definition at line 528 of file octree_search.hpp.
References pcl::octree::BRANCH_NODE, pcl::octree::OctreeNode::getNodeType(), pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
|
inlineprotected |
Find first child node ray will enter.
[in] | min_x | octree nodes X coordinate of lower bounding box corner |
[in] | min_y | octree nodes Y coordinate of lower bounding box corner |
[in] | min_z | octree nodes Z coordinate of lower bounding box corner |
[in] | mid_x | octree nodes X coordinate of bounding box mid line |
[in] | mid_y | octree nodes Y coordinate of bounding box mid line |
[in] | mid_z | octree nodes Z coordinate of bounding box mid line |
Definition at line 583 of file octree_search.h.
uindex_t pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::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).
[in] | origin | ray origin |
[in] | direction | ray direction vector |
[out] | voxel_center_list | results are written to this vector of PointT elements |
[in] | max_voxel_count | stop raycasting when this many voxels intersected (0: disable) |
Definition at line 603 of file octree_search.hpp.
References pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
|
protected |
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
[in] | min_x | octree nodes X coordinate of lower bounding box corner |
[in] | min_y | octree nodes Y coordinate of lower bounding box corner |
[in] | min_z | octree nodes Z coordinate of lower bounding box corner |
[in] | max_x | octree nodes X coordinate of upper bounding box corner |
[in] | max_y | octree nodes Y coordinate of upper bounding box corner |
[in] | max_z | octree nodes Z coordinate of upper bounding box corner |
[in] | a | number used for voxel child index remapping |
[in] | node | current octree node to be explored |
[in] | key | octree key addressing a leaf node. |
[out] | voxel_center_list | results are written to this vector of PointT elements |
[in] | max_voxel_count | stop raycasting when this many voxels intersected (0: disable) |
Definition at line 673 of file octree_search.hpp.
References pcl::octree::OctreeNode::getNodeType(), pcl::octree::LEAF_NODE, pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
uindex_t pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::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).
[in] | origin | ray origin |
[in] | direction | ray direction vector |
[out] | k_indices | resulting point indices from intersected voxels |
[in] | max_voxel_count | stop raycasting when this many voxels intersected (0: disable) |
Definition at line 639 of file octree_search.hpp.
References pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
Referenced by pcl::TextureMapping< PointInT >::removeOccludedPoints(), and pcl::TextureMapping< PointInT >::showOcclusions().
|
protected |
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
[in] | min_x | octree nodes X coordinate of lower bounding box corner |
[in] | min_y | octree nodes Y coordinate of lower bounding box corner |
[in] | min_z | octree nodes Z coordinate of lower bounding box corner |
[in] | max_x | octree nodes X coordinate of upper bounding box corner |
[in] | max_y | octree nodes Y coordinate of upper bounding box corner |
[in] | max_z | octree nodes Z coordinate of upper bounding box corner |
[in] | a | number used for voxel child index remapping |
[in] | node | current octree node to be explored |
[in] | key | octree key addressing a leaf node. |
[out] | k_indices | resulting indices |
[in] | max_voxel_count | stop raycasting when this many voxels intersected (0: disable) |
Definition at line 869 of file octree_search.hpp.
References pcl::octree::OctreeNode::getNodeType(), pcl::octree::LEAF_NODE, pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
|
protected |
Recursive search method that explores the octree and finds the K nearest neighbors.
[in] | point | query point |
[in] | K | amount of nearest neighbors to be found |
[in] | node | current octree node to be explored |
[in] | key | octree key addressing a leaf node. |
[in] | tree_depth | current depth/level in the octree |
[in] | squared_search_radius | squared search radius distance |
[out] | point_candidates | priority queue of nearest neighbor point candidates |
Definition at line 224 of file octree_search.hpp.
|
protected |
Recursive search method that explores the octree and finds neighbors within a given radius.
[in] | point | query point |
[in] | radiusSquared | squared search radius |
[in] | node | current octree node to be explored |
[in] | key | octree key addressing a leaf node. |
[in] | tree_depth | current depth/level in the octree |
[out] | k_indices | vector of indices found to be neighbors of query point |
[out] | k_sqr_distances | squared distances of neighbors to query point |
[in] | max_nn | maximum of neighbors to be found |
Definition at line 341 of file octree_search.hpp.
References pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
|
inlineprotected |
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.
[in] | x | current nodes X coordinate of upper bounding box corner |
[in] | y | current nodes Y coordinate of upper bounding box corner |
[in] | z | current nodes Z coordinate of upper bounding box corner |
[in] | a | next node if exit Plane YZ |
[in] | b | next node if exit Plane XZ |
[in] | c | next node if exit Plane XY |
Definition at line 641 of file octree_search.h.
|
inlineprotected |
Initialize raytracing algorithm.
[in] | origin | ray origin |
[in] | direction | ray direction vector |
[out] | min_x | octree nodes X coordinate of lower bounding box corner |
[out] | min_y | octree nodes Y coordinate of lower bounding box corner |
[out] | min_z | octree nodes Z coordinate of lower bounding box corner |
[out] | max_x | octree nodes X coordinate of upper bounding box corner |
[out] | max_y | octree nodes Y coordinate of upper bounding box corner |
[out] | max_z | octree nodes Z coordinate of upper bounding box corner |
[out] | a | number used for voxel child index remapping |
Definition at line 524 of file octree_search.h.
References pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_x_, pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_y_, pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_z_, pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_x_, pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_y_, and pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_z_.
|
inline |
Search for k-nearest neighbors at the query point.
[in] | cloud | the point cloud data |
[in] | index | the index in cloud representing the query point |
[in] | k | the number of neighbors to search for |
[out] | k_indices | the resultant indices of the neighboring points (must be resized to k a priori!) |
[out] | k_sqr_distances | the resultant squared distances to the neighboring points (must be resized to k a priori!) |
Definition at line 116 of file octree_search.h.
uindex_t pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::nearestKSearch | ( | const PointT & | p_q, |
uindex_t | k, | ||
Indices & | k_indices, | ||
std::vector< float > & | k_sqr_distances | ||
) |
Search for k-nearest neighbors at given query point.
[in] | p_q | the given query point |
[in] | k | the number of neighbors to search for |
[out] | k_indices | the resultant indices of the neighboring points (must be resized to k a priori!) |
[out] | k_sqr_distances | the resultant squared distances to the neighboring points (must be resized to k a priori!) |
Definition at line 82 of file octree_search.hpp.
References pcl::isFinite().
uindex_t pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::nearestKSearch | ( | uindex_t | index, |
uindex_t | k, | ||
Indices & | k_indices, | ||
std::vector< float > & | k_sqr_distances | ||
) |
Search for k-nearest neighbors at query point.
[in] | index | index representing the query point in the dataset given by setInputCloud. If indices were given in setInputCloud, index will be the position in the indices vector. |
[in] | k | the number of neighbors to search for |
[out] | k_indices | the resultant indices of the neighboring points (must be resized to k a priori!) |
[out] | k_sqr_distances | the resultant squared distances to the neighboring points (must be resized to k a priori!) |
Definition at line 126 of file octree_search.hpp.
|
protected |
Helper function to calculate the squared distance between two points.
[in] | point_a | point A |
[in] | point_b | point B |
Definition at line 520 of file octree_search.hpp.
|
inline |
Search for all neighbors of query point that are within a given radius.
[in] | cloud | the point cloud data |
[in] | index | the index in cloud representing the query point |
[in] | radius | the radius of the sphere bounding all of p_q's neighbors |
[out] | k_indices | the resultant indices of the neighboring points |
[out] | k_sqr_distances | the resultant squared distances to the neighboring points |
[in] | max_nn | if given, bounds the maximum returned neighbors to this value |
Definition at line 201 of file octree_search.h.
uindex_t pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::radiusSearch | ( | const PointT & | p_q, |
const double | radius, | ||
Indices & | k_indices, | ||
std::vector< float > & | k_sqr_distances, | ||
uindex_t | max_nn = 0 |
||
) | const |
Search for all neighbors of query point that are within a given radius.
[in] | p_q | the given query point |
[in] | radius | the radius of the sphere bounding all of p_q's neighbors |
[out] | k_indices | the resultant indices of the neighboring points |
[out] | k_sqr_distances | the resultant squared distances to the neighboring points |
[in] | max_nn | if given, bounds the maximum returned neighbors to this value |
Definition at line 163 of file octree_search.hpp.
References pcl::isFinite(), pcl::octree::OctreeKey::x, pcl::octree::OctreeKey::y, and pcl::octree::OctreeKey::z.
uindex_t pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::radiusSearch | ( | uindex_t | index, |
const double | radius, | ||
Indices & | k_indices, | ||
std::vector< float > & | k_sqr_distances, | ||
uindex_t | max_nn = 0 |
||
) | const |
Search for all neighbors of query point that are within a given radius.
[in] | index | index representing the query point in the dataset given by setInputCloud. If indices were given in setInputCloud, index will be the position in the indices vector |
[in] | radius | radius of the sphere bounding all of p_q's neighbors |
[out] | k_indices | the resultant indices of the neighboring points |
[out] | k_sqr_distances | the resultant squared distances to the neighboring points |
[in] | max_nn | if given, bounds the maximum returned neighbors to this value |
Definition at line 192 of file octree_search.hpp.
bool pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::voxelSearch | ( | const PointT & | point, |
Indices & | point_idx_data | ||
) |
Search for neighbors within a voxel at given point.
[in] | point | point addressing a leaf node voxel |
[out] | point_idx_data | the resultant indices of the neighboring voxel points |
Definition at line 50 of file octree_search.hpp.
References pcl::isFinite().
Referenced by pcl::GFPFHEstimation< PointInT, PointLT, PointOutT >::computeFeature().
bool pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::voxelSearch | ( | uindex_t | index, |
Indices & | point_idx_data | ||
) |
Search for neighbors within a voxel at given point referenced by a point index.
[in] | index | the index in input cloud defining the query point |
[out] | point_idx_data | the resultant indices of the neighboring voxel points |
Definition at line 73 of file octree_search.hpp.