template<typename PointT, typename LeafT, typename BranchT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::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, int maxVoxelCount) 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)
{
const LeafNode* leaf = static_cast<const LeafNode*> (node);
// decode leaf node into k_indices
leaf->getData (k_indices);
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 = static_cast<unsigned char> (currNode ^ a);
else
childIdx = a;
// childNode == 0 if childNode doesn't exist
childNode = this->getBranchChildPtr (static_cast<const BranchNode&> (*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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
currNode = 8;
break;
}
} while ((currNode < 8) && (maxVoxelCount <= 0 || voxelCount < maxVoxelCount));
return (voxelCount);
}
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);
}
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 LeafT, typename OctreeT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelCentersRecursive (
double minX, double minY, double minZ,
double maxX, double maxY, double maxZ,
unsigned char a, const OctreeNode* node, const OctreeKey& key,
AlignedPointTVector &voxelCenterList) 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)
{
PointT newPoint;
genLeafNodeCenterFromOctreeKey (key, newPoint);
voxelCenterList.push_back (newPoint);
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 = 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 += getIntersectedVoxelCentersRecursive (minX, minY, minZ, midX, midY, midZ, a,
childNode, childKey, voxelCenterList);
currNode = getNextIntersectedNode(midX, midY, midZ, 4, 2, 1);
break;
case 1:
if (childNode)
voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, midZ, midX, midY, maxZ, a,
childNode, childKey, voxelCenterList);
currNode = getNextIntersectedNode(midX, midY, maxZ, 5, 3, 8);
break;
case 2:
if (childNode)
voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, minZ, midX, maxY, midZ, a,
childNode, childKey, voxelCenterList);
currNode = getNextIntersectedNode(midX, maxY, midZ, 6, 8, 3);
break;
case 3:
if (childNode)
voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, midZ, midX, maxY, maxZ, a,
childNode, childKey, voxelCenterList);
currNode = getNextIntersectedNode(midX, maxY, maxZ, 7, 8, 8);
break;
case 4:
if (childNode)
voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, minZ, maxX, midY, midZ, a,
childNode, childKey, voxelCenterList);
currNode = getNextIntersectedNode(maxX, midY, midZ, 8, 6, 5);
break;
case 5:
if (childNode)
voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, midZ, maxX, midY, maxZ, a,
childNode, childKey, voxelCenterList);
currNode = getNextIntersectedNode(maxX, midY, maxZ, 8, 7, 8);
break;
case 6:
if (childNode)
voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, minZ, maxX, maxY, midZ, a,
childNode, childKey, voxelCenterList);
currNode = getNextIntersectedNode(maxX, maxY, midZ, 8, 8, 7);
break;
case 7:
if (childNode)
voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, midZ, maxX, maxY, maxZ, a,
childNode, childKey, voxelCenterList);
currNode = 8;
break;
}
}
while (currNode < 8);
return (voxelCount);
}