octree_search.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, 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_IMPL_H_
#define PCL_OCTREE_SEARCH_IMPL_H_
#include <cassert>
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch (const PointT& point,
std::vector<int>& point_idx_data)
{
assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
bool b_success = false;
// generate key
this->genOctreeKeyforPoint (point, key);
LeafContainerT* leaf = this->findLeaf (key);
if (leaf)
{
(*leaf).getPointIndices (point_idx_data);
b_success = true;
}
return (b_success);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch (const int index,
std::vector<int>& point_idx_data)
{
const PointT search_point = this->getPointByIndex (index);
return (this->voxelSearch (search_point, point_idx_data));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch (const PointT &p_q, int k,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances)
{
assert(this->leaf_count_>0);
assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
k_indices.clear ();
k_sqr_distances.clear ();
if (k < 1)
return 0;
prioPointQueueEntry point_entry;
std::vector<prioPointQueueEntry> point_candidates;
OctreeKey key;
key.x = key.y = key.z = 0;
// initialize smallest point distance in search with high value
double smallest_dist = std::numeric_limits<double>::max ();
getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
unsigned int result_count = static_cast<unsigned int> (point_candidates.size ());
k_indices.resize (result_count);
k_sqr_distances.resize (result_count);
for (unsigned int i = 0; i < result_count; ++i)
{
k_indices [i] = point_candidates [i].point_idx_;
k_sqr_distances [i] = point_candidates [i].point_distance_;
}
return static_cast<int> (k_indices.size ());
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch (int index, int k,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances)
{
const PointT search_point = this->getPointByIndex (index);
return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch (const PointT &p_q,
int &result_index,
float &sqr_distance)
{
assert(this->leaf_count_>0);
assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
key.x = key.y = key.z = 0;
approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance);
return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch (int query_index, int &result_index,
float &sqr_distance)
{
const PointT search_point = this->getPointByIndex (query_index);
return (approxNearestSearch (search_point, result_index, sqr_distance));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch (const PointT &p_q, const double radius,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear ();
k_sqr_distances.clear ();
getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances,
max_nn);
return (static_cast<int> (k_indices.size ()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch (int index, const double radius,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
const PointT search_point = this->getPointByIndex (index);
return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearch (const Eigen::Vector3f &min_pt,
const Eigen::Vector3f &max_pt,
std::vector<int> &k_indices) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear ();
boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices);
return (static_cast<int> (k_indices.size ()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> double
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::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
{
std::vector<prioBranchQueueEntry> search_heap;
search_heap.resize (8);
OctreeKey new_key;
double smallest_squared_dist = squared_search_radius;
// get spatial voxel information
double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth);
// iterate over all children
for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
{
if (this->branchHasChild (*node, child_idx))
{
PointT voxel_center;
search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center);
// generate new priority queue element
search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx);
search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point);
}
else
{
search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity ();
}
}
std::sort (search_heap.begin (), search_heap.end ());
// iterate over all children in priority queue
// check if the distance to search candidate is smaller than the best point distance (smallest_squared_dist)
while ((!search_heap.empty ()) && (search_heap.back ().point_distance <
smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_))
{
const OctreeNode* child_node;
// read from priority queue element
child_node = search_heap.back ().node;
new_key = search_heap.back ().key;
if (tree_depth < this->octree_depth_)
{
// we have not reached maximum tree depth
smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
smallest_squared_dist, point_candidates);
}
else
{
// we reached leaf node level
std::vector<int> decoded_point_vector;
const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
// decode leaf node into decoded_point_vector
(*child_leaf)->getPointIndices (decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
for (const int &point_index : decoded_point_vector)
{
const PointT& candidate_point = this->getPointByIndex (point_index);
// calculate point distance to search point
float squared_dist = pointSquaredDist (candidate_point, point);
// check if a closer match is found
if (squared_dist < smallest_squared_dist)
{
prioPointQueueEntry point_entry;
point_entry.point_distance_ = squared_dist;
point_entry.point_idx_ = point_index;
point_candidates.push_back (point_entry);
}
}
std::sort (point_candidates.begin (), point_candidates.end ());
if (point_candidates.size () > K)
point_candidates.resize (K);
if (point_candidates.size () == K)
smallest_squared_dist = point_candidates.back ().point_distance_;
}
// pop element from priority queue
search_heap.pop_back ();
}
return (smallest_squared_dist);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::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
{
// get spatial voxel information
double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth);
// iterate over all children
for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
{
if (!this->branchHasChild (*node, child_idx))
continue;
const OctreeNode* child_node;
child_node = this->getBranchChildPtr (*node, child_idx);
OctreeKey new_key;
PointT voxel_center;
float squared_dist;
// generate new key for current branch voxel
new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
// calculate distance to search point
squared_dist = pointSquaredDist (static_cast<const PointT&> (voxel_center), point);
// if distance is smaller than search radius
if (squared_dist + this->epsilon_
<= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared))
{
if (tree_depth < this->octree_depth_)
{
// we have not reached maximum tree depth
getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
k_indices, k_sqr_distances, max_nn);
if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
return;
}
else
{
// we reached leaf node level
const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
std::vector<int> decoded_point_vector;
// decode leaf node into decoded_point_vector
(*child_leaf)->getPointIndices (decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
for (const int &index : decoded_point_vector)
{
const PointT& candidate_point = this->getPointByIndex (index);
// calculate point distance to search point
squared_dist = pointSquaredDist (candidate_point, point);
// check if a match is found
if (squared_dist > radiusSquared)
continue;
// add point to result vector
k_indices.push_back (index);
k_sqr_distances.push_back (squared_dist);
if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
return;
}
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearchRecursive (const PointT & point,
const BranchNode* node,
const OctreeKey& key,
unsigned int tree_depth,
int& result_index,
float& sqr_distance)
{
OctreeKey minChildKey;
OctreeKey new_key;
const OctreeNode* child_node;
// set minimum voxel distance to maximum value
double min_voxel_center_distance = std::numeric_limits<double>::max ();
unsigned char min_child_idx = 0xFF;
// iterate over all children
for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
{
if (!this->branchHasChild (*node, child_idx))
continue;
PointT voxel_center;
double voxelPointDist;
new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
voxelPointDist = pointSquaredDist (voxel_center, point);
// search for child voxel with shortest distance to search point
if (voxelPointDist >= min_voxel_center_distance)
continue;
min_voxel_center_distance = voxelPointDist;
min_child_idx = child_idx;
minChildKey = new_key;
}
// make sure we found at least one branch child
assert(min_child_idx<8);
child_node = this->getBranchChildPtr (*node, min_child_idx);
if (tree_depth < this->octree_depth_)
{
// we have not reached maximum tree depth
approxNearestSearchRecursive (point, static_cast<const BranchNode*> (child_node), minChildKey, tree_depth + 1, result_index,
sqr_distance);
}
else
{
// we reached leaf node level
std::vector<int> decoded_point_vector;
const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
double smallest_squared_dist = std::numeric_limits<double>::max ();
// decode leaf node into decoded_point_vector
(**child_leaf).getPointIndices (decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
for (const int &index : decoded_point_vector)
{
const PointT& candidate_point = this->getPointByIndex (index);
// calculate point distance to search point
double squared_dist = pointSquaredDist (candidate_point, point);
// check if a closer match is found
if (squared_dist >= smallest_squared_dist)
continue;
result_index = index;
smallest_squared_dist = squared_dist;
sqr_distance = static_cast<float> (squared_dist);
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> float
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::pointSquaredDist (const PointT & point_a,
const PointT & point_b) const
{
return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::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
{
// iterate over all children
for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
{
const OctreeNode* child_node;
child_node = this->getBranchChildPtr (*node, child_idx);
if (!child_node)
continue;
OctreeKey new_key;
// generate new key for current branch voxel
new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// voxel corners
Eigen::Vector3f lower_voxel_corner;
Eigen::Vector3f upper_voxel_corner;
// get voxel coordinates
this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
// test if search region overlap with voxel space
if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) ||
(lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) ||
(lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) )
{
if (tree_depth < this->octree_depth_)
{
// we have not reached maximum tree depth
boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices);
}
else
{
// we reached leaf node level
std::vector<int> decoded_point_vector;
bool bInBox;
const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
// decode leaf node into decoded_point_vector
(**child_leaf).getPointIndices (decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
for (const int &index : decoded_point_vector)
{
const PointT& candidate_point = this->getPointByIndex (index);
// check if point falls within search box
bInBox = ( (candidate_point.x >= min_pt (0)) && (candidate_point.x <= max_pt (0)) &&
(candidate_point.y >= min_pt (1)) && (candidate_point.y <= max_pt (1)) &&
(candidate_point.z >= min_pt (2)) && (candidate_point.z <= max_pt (2)) );
if (bInBox)
// add to result vector
k_indices.push_back (index);
}
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelCenters (
Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list,
int max_voxel_count) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
voxel_center_list.clear ();
// Voxel child_idx remapping
unsigned char a = 0;
double min_x, min_y, min_z, max_x, max_y, max_z;
initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
voxel_center_list, max_voxel_count);
return (0);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelIndices (
Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
int max_voxel_count) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear ();
// Voxel child_idx remapping
unsigned char a = 0;
double min_x, min_y, min_z, max_x, max_y, max_z;
initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
k_indices, max_voxel_count);
return (0);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::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
{
if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
return (0);
// If leaf node, get voxel center and increment intersection count
if (node->getNodeType () == LEAF_NODE)
{
PointT newPoint;
this->genLeafNodeCenterFromOctreeKey (key, newPoint);
voxel_center_list.push_back (newPoint);
return (1);
}
// Voxel intersection count for branches children
int voxel_count = 0;
// Voxel mid lines
double mid_x = 0.5 * (min_x + max_x);
double mid_y = 0.5 * (min_y + max_y);
double mid_z = 0.5 * (min_z + max_z);
// First voxel node ray will intersect
int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
// Child index, node and key
unsigned char child_idx;
const OctreeNode *child_node;
OctreeKey child_key;
do
{
if (curr_node != 0)
child_idx = static_cast<unsigned char> (curr_node ^ a);
else
child_idx = a;
// child_node == 0 if child_node doesn't exist
child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
// Generate new key for current branch voxel
child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
// Recursively call each intersected child node, selecting the next
// node intersected by the ray. Children that do not intersect will
// not be traversed.
switch (curr_node)
{
case 0:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
break;
case 1:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
break;
case 2:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
break;
case 3:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
break;
case 4:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
break;
case 5:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
break;
case 6:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
break;
case 7:
if (child_node)
voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
child_key, voxel_center_list, max_voxel_count);
curr_node = 8;
break;
}
} while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
return (voxel_count);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::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
{
if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
return (0);
// If leaf node, get voxel center and increment intersection count
if (node->getNodeType () == LEAF_NODE)
{
const LeafNode* leaf = static_cast<const LeafNode*> (node);
// decode leaf node into k_indices
(*leaf)->getPointIndices (k_indices);
return (1);
}
// Voxel intersection count for branches children
int voxel_count = 0;
// Voxel mid lines
double mid_x = 0.5 * (min_x + max_x);
double mid_y = 0.5 * (min_y + max_y);
double mid_z = 0.5 * (min_z + max_z);
// First voxel node ray will intersect
int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
// Child index, node and key
unsigned char child_idx;
const OctreeNode *child_node;
OctreeKey child_key;
do
{
if (curr_node != 0)
child_idx = static_cast<unsigned char> (curr_node ^ a);
else
child_idx = a;
// child_node == 0 if child_node doesn't exist
child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
// Generate new key for current branch voxel
child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
// Recursively call each intersected child node, selecting the next
// node intersected by the ray. Children that do not intersect will
// not be traversed.
switch (curr_node)
{
case 0:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
break;
case 1:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
break;
case 2:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
break;
case 3:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
break;
case 4:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
break;
case 5:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
break;
case 6:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
break;
case 7:
if (child_node)
voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
child_key, k_indices, max_voxel_count);
curr_node = 8;
break;
}
} while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
return (voxel_count);
}
#define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
#endif // PCL_OCTREE_SEARCH_IMPL_H_
pcl::K
Definition: norms.h:54
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:724
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:583
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
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:205
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:485
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:46
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:112
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:309
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:558
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:50
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:182
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:149
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:67
pcl::octree::LEAF_NODE
Definition: octree_nodes.h:59
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:96
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:203
pcl::octree::OctreeKey::z
std::uint32_t z
Definition: octree_key.h:154
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:389
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:477
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:606
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:102
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 Points exactly on the edges of the search rectangle ...
Definition: octree_search.hpp:187
pcl::octree::OctreeKey::x
std::uint32_t x
Definition: octree_key.h:152
pcl::octree::OctreeKey::y
std::uint32_t y
Definition: octree_key.h:153