int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelCenters (
Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxelCenterList) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
voxelCenterList.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 getIntersectedVoxelCentersRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key,
voxelCenterList);
return (0);
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelCenters (
Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list,
int max_voxel_count) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
voxel_center_list.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 getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
voxel_center_list, max_voxel_count);
return (0);
}
template<typename PointT, typename LeafT, typename BranchT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::getIntersectedVoxelCentersRecursive (
double minX, double minY, double minZ, double maxX, double maxY, double maxZ, unsigned char a,
const OctreeNode* node, const OctreeKey& key, AlignedPointTVector &voxelCenterList, 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)
{
PointT newPoint;
this->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 = 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 += getIntersectedVoxelCentersRecursive (minX, minY, minZ, midX, midY, midZ, a, childNode,
childKey, voxelCenterList, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
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, maxVoxelCount);
currNode = 8;
break;
}
} while ((currNode < 8) && (maxVoxelCount <= 0 || voxelCount < maxVoxelCount));
return (voxelCount);
}
