void
OctreeLowMemBase<DataT, LeafT>::deserializeLeafCallback (
OctreeLeaf& leaf_arg,
const OctreeKey& key_arg,
typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg)
{
OctreeKey dataKey;
bool bKeyBasedEncoding = false;
// add DataT objects to octree leaf as long as their key fit to voxel
while ((dataVectorIterator_arg != dataVectorEndIterator_arg)
&& (this->genOctreeKeyForDataT (*dataVectorIterator_arg, dataKey) && (dataKey == key_arg)))
{
leaf_arg.setData (*dataVectorIterator_arg);
dataVectorIterator_arg++;
bKeyBasedEncoding = true;
}
// add single DataT object to octree if key-based encoding is disabled
if (!bKeyBasedEncoding)
{
leaf_arg.setData (*dataVectorIterator_arg);
dataVectorIterator_arg++;
}
}
void
OctreeLowMemBase<DataT, LeafT>::deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg)
{
DataT newDataT;
// initialize new leaf child
if (genDataTByOctreeKey (key_arg, newDataT))
{
leaf_arg.setData (newDataT);
}
}
void
OctreeLowMemBase<DataT, LeafT>::deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg,
const OctreeKey& key_arg,
std::vector<DataT>& dataVector_arg)
{
DataT newDataT;
// initialize new leaf child
if (genDataTByOctreeKey (key_arg, newDataT))
{
leaf_arg.setData (newDataT);
dataVector_arg.push_back (newDataT);
}
}
int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelIndicesRecursive (
double minX, double minY, double minZ, double maxX, double maxY, double maxZ, unsigned char a,
const OctreeNode* node, const OctreeKey& key, std::vector<int> &k_indices) const
{
if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0)
return (0);
// If leaf node, get voxel center and increment intersection count
if (node->getNodeType () == LEAF_NODE)
{
OctreeLeaf* leaf = (OctreeLeaf*)node;
vector<int> indices;
// decode leaf node into decodedPointVector
leaf->getData (indices);
for (size_t i = 0; i < indices.size (); i++)
{
k_indices.push_back (indices[i]);
}
return (1);
}
// Voxel intersection count for branches children
int voxelCount = 0;
// Voxel mid lines
double midX = 0.5 * (minX + maxX);
double midY = 0.5 * (minY + maxY);
double midZ = 0.5 * (minZ + maxZ);
// First voxel node ray will intersect
int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ);
// Child index, node and key
unsigned char childIdx;
const OctreeNode *childNode;
OctreeKey childKey;
do
{
if (currNode != 0)
childIdx = currNode ^ a;
else
childIdx = a;
// childNode == 0 if childNode doesn't exist
childNode = this->getBranchChild ((OctreeBranch&)*node, childIdx);
// Generate new key for current branch voxel
childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2)));
childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1)));
childKey.z = (key.z << 1) | (!!(childIdx & (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 (currNode)
{
case 0:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, minZ, midX, midY, midZ, a, childNode,
childKey, k_indices);
currNode = getNextIntersectedNode (midX, midY, midZ, 4, 2, 1);
break;
case 1:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, midZ, midX, midY, maxZ, a, childNode,
childKey, k_indices);
currNode = getNextIntersectedNode (midX, midY, maxZ, 5, 3, 8);
break;
case 2:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, minZ, midX, maxY, midZ, a, childNode,
childKey, k_indices);
currNode = getNextIntersectedNode (midX, maxY, midZ, 6, 8, 3);
break;
case 3:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, midZ, midX, maxY, maxZ, a, childNode,
childKey, k_indices);
currNode = getNextIntersectedNode (midX, maxY, maxZ, 7, 8, 8);
break;
case 4:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, minZ, maxX, midY, midZ, a, childNode,
childKey, k_indices);
currNode = getNextIntersectedNode (maxX, midY, midZ, 8, 6, 5);
break;
case 5:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, midZ, maxX, midY, maxZ, a, childNode,
childKey, k_indices);
currNode = getNextIntersectedNode (maxX, midY, maxZ, 8, 7, 8);
break;
case 6:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, minZ, maxX, maxY, midZ, a, childNode,
childKey, k_indices);
currNode = getNextIntersectedNode (maxX, maxY, midZ, 8, 8, 7);
break;
case 7:
if (childNode)
voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, midZ, maxX, maxY, maxZ, a, childNode,
childKey, k_indices);
currNode = 8;
break;
}
} while (currNode < 8);
return (voxelCount);
}
void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::boxSearchRecursive (const Eigen::Vector3f &min_pt,
const Eigen::Vector3f &max_pt,
const OctreeBranch* 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++)
{
if (!this->branchHasChild (*node, childIdx))
continue;
const OctreeNode* childNode;
childNode = this->getBranchChild (*node, childIdx);
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, (OctreeBranch*)childNode, newKey, treeDepth + 1, k_indices);
}
else
{
// we reached leaf node level
size_t i;
vector<int> decodedPointVector;
bool bInBox;
OctreeLeaf* childLeaf = (OctreeLeaf*)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]);
}
}
}
}
}
void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::approxNearestSearchRecursive (const PointT & point,
const OctreeBranch* 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->getBranchChild (*node, minChildIdx);
if (treeDepth < this->octreeDepth_)
{
// we have not reached maximum tree depth
approxNearestSearchRecursive (point, (OctreeBranch*)childNode, minChildKey, treeDepth + 1, result_index,
sqr_distance);
}
else
{
// we reached leaf node level
double squaredDist;
double smallestSquaredDist;
size_t i;
vector<int> decodedPointVector;
OctreeLeaf* childLeaf = (OctreeLeaf*)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];
sqr_distance = smallestSquaredDist = squaredDist;
}
}
}
void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getNeighborsWithinRadiusRecursive (
const PointT & point, const double radiusSquared, const OctreeBranch* 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->getBranchChild (*node, childIdx);
OctreeKey newKey;
PointT voxelCenter;
double 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 ((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, (OctreeBranch*)childNode, newKey, treeDepth + 1,
k_indices, k_sqr_distances, max_nn);
if (max_nn != 0 && k_indices.size () == (unsigned int)max_nn)
return;
}
else
{
// we reached leaf node level
size_t i;
OctreeLeaf* childLeaf = (OctreeLeaf*)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 () == (unsigned int)max_nn)
return;
}
}
}
}
}
double
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getKNearestNeighborRecursive (
const PointT & point, unsigned int K, const OctreeBranch* node, const OctreeKey& key, unsigned int treeDepth,
const double squaredSearchRadius, std::vector<prioPointQueueEntry>& pointCandidates) const
{
std::vector<prioBranchQueueEntry> searchEntryHeap;
searchEntryHeap.resize (8);
unsigned char childIdx;
OctreeKey newKey;
double smallestSquaredDist = squaredSearchRadius;
// get spatial voxel information
double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth);
// iterate over all children
for (childIdx = 0; childIdx < 8; childIdx++)
{
if (this->branchHasChild (*node, childIdx))
{
PointT voxelCenter;
searchEntryHeap[childIdx].key.x = (key.x << 1) + (!!(childIdx & (1 << 2)));
searchEntryHeap[childIdx].key.y = (key.y << 1) + (!!(childIdx & (1 << 1)));
searchEntryHeap[childIdx].key.z = (key.z << 1) + (!!(childIdx & (1 << 0)));
// generate voxel center point for voxel at key
this->genVoxelCenterFromOctreeKey (searchEntryHeap[childIdx].key, treeDepth, voxelCenter);
// generate new priority queue element
searchEntryHeap[childIdx].node = this->getBranchChild (*node, childIdx);
searchEntryHeap[childIdx].pointDistance = pointSquaredDist (voxelCenter, point);
}
else
{
searchEntryHeap[childIdx].pointDistance = numeric_limits<double>::infinity ();
}
}
std::sort (searchEntryHeap.begin (), searchEntryHeap.end ());
// iterate over all children in priority queue
// check if the distance to seach candidate is smaller than the best point distance (smallestSquaredDist)
while ((!searchEntryHeap.empty ())
&& (searchEntryHeap.back ().pointDistance
< smallestSquaredDist + voxelSquaredDiameter / 4.0 + sqrt (smallestSquaredDist * voxelSquaredDiameter)
- this->epsilon_))
{
const OctreeNode* childNode;
// read from priority queue element
childNode = searchEntryHeap.back ().node;
newKey = searchEntryHeap.back ().key;
if (treeDepth < this->octreeDepth_)
{
// we have not reached maximum tree depth
smallestSquaredDist = getKNearestNeighborRecursive (point, K, (OctreeBranch*)childNode, newKey, treeDepth + 1,
smallestSquaredDist, pointCandidates);
}
else
{
// we reached leaf node level
double squaredDist;
size_t i;
vector<int> decodedPointVector;
OctreeLeaf* childLeaf = (OctreeLeaf*)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]);
// calculate point distance to search point
squaredDist = pointSquaredDist (candidatePoint, point);
// check if a closer match is found
if (squaredDist < smallestSquaredDist)
{
prioPointQueueEntry pointEntry;
pointEntry.pointDistance_ = squaredDist;
pointEntry.pointIdx_ = decodedPointVector[i];
pointCandidates.push_back (pointEntry);
}
}
std::sort (pointCandidates.begin (), pointCandidates.end ());
if (pointCandidates.size () > K)
pointCandidates.resize (K);
if (pointCandidates.size () == K)
smallestSquaredDist = pointCandidates.back ().pointDistance_;
}
// pop element from priority queue
searchEntryHeap.pop_back ();
}
return (smallestSquaredDist);
}
void
OctreeLowMemBase<DataT, LeafT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg,
std::vector<DataT>& dataVector_arg)
{
leaf_arg.getData (dataVector_arg);
}
