41 #include <pcl/octree/octree_pointcloud.h>
42 #include <pcl/point_cloud.h>
55 typename LeafContainerT = OctreeContainerPointIndices,
56 typename BranchContainerT = OctreeContainerEmpty>
70 shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
103 voxelSearch(
const int index, std::vector<int>& point_idx_data);
119 std::vector<int>& k_indices,
120 std::vector<float>& k_sqr_distances)
122 return (
nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
137 std::vector<int>& k_indices,
138 std::vector<float>& k_sqr_distances);
154 std::vector<int>& k_indices,
155 std::vector<float>& k_sqr_distances);
206 std::vector<int>& k_indices,
207 std::vector<float>& k_sqr_distances,
208 unsigned int max_nn = 0)
210 return (
radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
225 std::vector<int>& k_indices,
226 std::vector<float>& k_sqr_distances,
227 unsigned int max_nn = 0)
const;
243 std::vector<int>& k_indices,
244 std::vector<float>& k_sqr_distances,
245 unsigned int max_nn = 0)
const;
258 Eigen::Vector3f direction,
260 int max_voxel_count = 0)
const;
272 Eigen::Vector3f direction,
273 std::vector<int>& k_indices,
274 int max_voxel_count = 0)
const;
285 const Eigen::Vector3f& max_pt,
286 std::vector<int>& k_indices)
const;
388 const double radiusSquared,
391 unsigned int tree_depth,
392 std::vector<int>& k_indices,
393 std::vector<float>& k_sqr_distances,
394 unsigned int max_nn)
const;
413 unsigned int tree_depth,
414 const double squared_search_radius,
415 std::vector<prioPointQueueEntry>& point_candidates)
const;
430 unsigned int tree_depth,
432 float& sqr_distance);
463 int max_voxel_count)
const;
476 const Eigen::Vector3f& max_pt,
479 unsigned int tree_depth,
480 std::vector<int>& k_indices)
const;
509 std::vector<int>& k_indices,
510 int max_voxel_count)
const;
525 Eigen::Vector3f& direction,
532 unsigned char& a)
const
535 const float epsilon = 1e-10f;
536 if (direction.x() == 0.0)
537 direction.x() = epsilon;
538 if (direction.y() == 0.0)
539 direction.y() = epsilon;
540 if (direction.z() == 0.0)
541 direction.z() = epsilon;
547 if (direction.x() < 0.0) {
548 origin.x() =
static_cast<float>(this->
min_x_) +
static_cast<float>(this->
max_x_) -
550 direction.x() = -direction.x();
553 if (direction.y() < 0.0) {
554 origin.y() =
static_cast<float>(this->
min_y_) +
static_cast<float>(this->
max_y_) -
556 direction.y() = -direction.y();
559 if (direction.z() < 0.0) {
560 origin.z() =
static_cast<float>(this->
min_z_) +
static_cast<float>(this->
max_z_) -
562 direction.z() = -direction.z();
565 min_x = (this->
min_x_ - origin.x()) / direction.x();
566 max_x = (this->
max_x_ - origin.x()) / direction.x();
567 min_y = (this->
min_y_ - origin.y()) / direction.y();
568 max_y = (this->
max_y_ - origin.y()) / direction.y();
569 min_z = (this->
min_z_ - origin.z()) / direction.z();
570 max_z = (this->
max_z_ - origin.z()) / direction.z();
656 #ifdef PCL_NO_PRECOMPILE
657 #include <pcl/octree/impl/octree_search.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Abstract octree node class
typename OctreeT::LeafNode LeafNode
typename OctreeT::BranchNode BranchNode
Priority queue entry for branch nodes
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
const OctreeNode * node
Pointer to octree node.
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
prioBranchQueueEntry()
Empty constructor
float point_distance
Distance to query point.
Priority queue entry for point candidates
int point_idx_
Index representing a point in the dataset given by setInputCloud.
prioPointQueueEntry()
Empty constructor
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
float point_distance_
Distance to query point.
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
Octree pointcloud search class
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.
typename OctreeT::LeafNode LeafNode
shared_ptr< std::vector< int > > IndicesPtr
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.
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.
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.
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.
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.
shared_ptr< const std::vector< int > > IndicesConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
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.
shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
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.
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
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.
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).
void initIntersectedVoxel(Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
Initialize raytracing algorithm.
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
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 Points exactly on the edges of the search rectangle ...
typename OctreeT::BranchNode BranchNode
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.
OctreePointCloudSearch(const double resolution)
Constructor.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
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).
typename PointCloud::Ptr PointCloudPtr
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
A point structure representing Euclidean xyz coordinates, and the RGB color.