int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelIndices (
Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear ();
// Voxel childIdx remapping
unsigned char a = 0;
double minX, minY, minZ, maxX, maxY, maxZ;
initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a);
if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ))
return getIntersectedVoxelIndicesRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key,
k_indices);
return (0);
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelIndices (
Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
int max_voxel_count) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear ();
// Voxel child_idx remapping
unsigned char a = 0;
double min_x, min_y, min_z, max_x, max_y, max_z;
initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
k_indices, max_voxel_count);
return (0);
}
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);
}
