template<typename PointT, typename LeafT, typename OctreeT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::approxNearestSearch (
const PointT &p_q, int &result_index, float &sqr_distance)
{
assert (this->leafCount_>0);
OctreeKey key;
key.x = key.y = key.z = 0;
approxNearestSearchRecursive (p_q, this->rootNode_, key, 1, result_index, sqr_distance);
}
template<typename PointT, typename LeafT, typename BranchT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::approxNearestSearch (const PointT &p_q,
int &result_index,
float &sqr_distance)
{
assert(this->leafCount_>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->rootNode_, key, 1, result_index, sqr_distance);
return;
}
template<typename PointT, typename LeafT, typename BranchT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::approxNearestSearchRecursive (const PointT & point,
const BranchNode* node,
const OctreeKey& key,
unsigned int treeDepth,
int& result_index,
float& sqr_distance)
{
unsigned char childIdx;
unsigned char minChildIdx;
double minVoxelCenterDistance;
OctreeKey minChildKey;
OctreeKey newKey;
const OctreeNode* childNode;
// set minimum voxel distance to maximum value
minVoxelCenterDistance = numeric_limits<double>::max ();
minChildIdx = 0xFF;
// iterate over all children
for (childIdx = 0; childIdx < 8; childIdx++)
{
if (!this->branchHasChild (*node, childIdx))
continue;
PointT voxelCenter;
double voxelPointDist;
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);
voxelPointDist = pointSquaredDist (voxelCenter, point);
// search for child voxel with shortest distance to search point
if (voxelPointDist >= minVoxelCenterDistance)
continue;
minVoxelCenterDistance = voxelPointDist;
minChildIdx = childIdx;
minChildKey = newKey;
}
// make sure we found at least one branch child
assert(minChildIdx<8);
childNode = this->getBranchChildPtr (*node, minChildIdx);
if (treeDepth < this->octreeDepth_)
{
// we have not reached maximum tree depth
approxNearestSearchRecursive (point, static_cast<const BranchNode*> (childNode), minChildKey, treeDepth + 1, result_index,
sqr_distance);
}
else
{
// we reached leaf node level
double squaredDist;
double smallestSquaredDist;
size_t i;
vector<int> decodedPointVector;
const LeafNode* childLeaf = static_cast<const LeafNode*> (childNode);
smallestSquaredDist = numeric_limits<double>::max ();
// 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 closer match is found
if (squaredDist >= smallestSquaredDist)
continue;
result_index = decodedPointVector[i];
smallestSquaredDist = squaredDist;
sqr_distance = static_cast<float> (squaredDist);
}
}
}
