template<typename PointT, typename LeafT, typename OctreeT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::radiusSearch (
const PointT &p_q, const double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, int max_nn) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear ();
k_sqr_distances.clear ();
getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->rootNode_, key, 1, k_indices,
k_sqr_distances, max_nn);
return (k_indices.size ());
}
template<typename PointT, typename LeafT, typename BranchT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::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->rootNode_, key, 1, k_indices, k_sqr_distances,
max_nn);
return (static_cast<int> (k_indices.size ()));
}
template<typename PointT, typename LeafT, typename BranchT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::getNeighborsWithinRadiusRecursive (
const PointT & point, const double radiusSquared, const BranchNode* node, const OctreeKey& key,
unsigned int treeDepth, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances,
unsigned int max_nn) const
{
// child iterator
unsigned char childIdx;
// get spatial voxel information
double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth);
// iterate over all children
for (childIdx = 0; childIdx < 8; childIdx++)
{
if (!this->branchHasChild (*node, childIdx))
continue;
const OctreeNode* childNode;
childNode = this->getBranchChildPtr (*node, childIdx);
OctreeKey newKey;
PointT voxelCenter;
float squaredDist;
// 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)));
// generate voxel center point for voxel at key
this->genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter);
// calculate distance to search point
squaredDist = pointSquaredDist (static_cast<const PointT&> (voxelCenter), point);
// if distance is smaller than search radius
if (squaredDist + this->epsilon_
<= voxelSquaredDiameter / 4.0 + radiusSquared + sqrt (voxelSquaredDiameter * radiusSquared))
{
if (treeDepth < this->octreeDepth_)
{
// we have not reached maximum tree depth
getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast<const BranchNode*> (childNode), newKey, treeDepth + 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
size_t i;
const LeafNode* childLeaf = static_cast<const LeafNode*> (childNode);
vector<int> decodedPointVector;
// 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]);
// calculate point distance to search point
squaredDist = pointSquaredDist (candidatePoint, point);
// check if a match is found
if (squaredDist > radiusSquared)
continue;
// add point to result vector
k_indices.push_back (decodedPointVector[i]);
k_sqr_distances.push_back (squaredDist);
if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
return;
}
}
}
}
}
