template<typename PointT, typename LeafT, typename BranchT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::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->rootNode_, key, 1, k_indices);
return (static_cast<int> (k_indices.size ()));
}
int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::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->rootNode_, key, 1, k_indices);
return (k_indices.size ());
}
template<typename PointT, typename LeafT, typename BranchT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::boxSearchRecursive (const Eigen::Vector3f &min_pt,
const Eigen::Vector3f &max_pt,
const BranchNode* node,
const OctreeKey& key,
unsigned int treeDepth,
std::vector<int>& k_indices) const
{
// child iterator
unsigned char childIdx;
// iterate over all children
for (childIdx = 0; childIdx < 8; childIdx++)
{
const OctreeNode* childNode;
childNode = this->getBranchChildPtr (*node, childIdx);
if (!childNode)
continue;
OctreeKey newKey;
// generate new key for current branch voxel
newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2)));
newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1)));
newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0)));
// voxel corners
Eigen::Vector3f lowerVoxelCorner;
Eigen::Vector3f upperVoxelCorner;
// get voxel coordinates
this->genVoxelBoundsFromOctreeKey (newKey, treeDepth, lowerVoxelCorner, upperVoxelCorner);
// test if search region overlap with voxel space
if ( !( (lowerVoxelCorner (0) > max_pt (0)) || (min_pt (0) > upperVoxelCorner(0)) ||
(lowerVoxelCorner (1) > max_pt (1)) || (min_pt (1) > upperVoxelCorner(1)) ||
(lowerVoxelCorner (2) > max_pt (2)) || (min_pt (2) > upperVoxelCorner(2)) ) )
{
if (treeDepth < this->octreeDepth_)
{
// we have not reached maximum tree depth
boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (childNode), newKey, treeDepth + 1, k_indices);
}
else
{
// we reached leaf node level
size_t i;
vector<int> decodedPointVector;
bool bInBox;
const LeafNode* childLeaf = static_cast<const LeafNode*> (childNode);
// decode leaf node into decodedPointVector
childLeaf->getData (decodedPointVector);
// Linearly iterate over all decoded (unsorted) points
for (i = 0; i < decodedPointVector.size (); i++)
{
const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]);
// check if point falls within search box
bInBox = ( (candidatePoint.x > min_pt (0)) && (candidatePoint.x < max_pt (0)) &&
(candidatePoint.y > min_pt (1)) && (candidatePoint.y < max_pt (1)) &&
(candidatePoint.z > min_pt (2)) && (candidatePoint.z < max_pt (2)) );
if (bInBox)
// add to result vector
k_indices.push_back (decodedPointVector[i]);
}
}
}
}
}
