template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
const Eigen::Vector3f& origin,
const Eigen::Vector3f& end,
AlignedPointTVector &voxel_center_list,
float precision)
{
Eigen::Vector3f direction = end - origin;
float norm = direction.norm ();
direction.normalize ();
const float step_size = static_cast<const float> (resolution_) * precision;
// Ensure we get at least one step for the first voxel.
const int nsteps = std::max (1, static_cast<int> (norm / step_size));
OctreeKey prev_key;
bool bkeyDefined = false;
// Walk along the line segment with small steps.
for (int i = 0; i < nsteps; ++i)
{
Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
PointT octree_p;
octree_p.x = p.x ();
octree_p.y = p.y ();
octree_p.z = p.z ();
OctreeKey key;
this->genOctreeKeyforPoint (octree_p, key);
// Not a new key, still the same voxel.
if ((key == prev_key) && (bkeyDefined) )
continue;
prev_key = key;
bkeyDefined = true;
PointT center;
genLeafNodeCenterFromOctreeKey (key, center);
voxel_center_list.push_back (center);
}
OctreeKey end_key;
PointT end_p;
end_p.x = end.x ();
end_p.y = end.y ();
end_p.z = end.z ();
this->genOctreeKeyforPoint (end_p, end_key);
if (!(end_key == prev_key))
{
PointT center;
genLeafNodeCenterFromOctreeKey (end_key, center);
voxel_center_list.push_back (center);
}
return (static_cast<int> (voxel_center_list.size ()));
}
template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getOccupiedVoxelCentersRecursive (
const BranchNode* node_arg,
const OctreeKey& key_arg,
AlignedPointTVector &voxelCenterList_arg) const
{
// child iterator
unsigned char childIdx;
int voxelCount = 0;
// iterate over all children
for (childIdx = 0; childIdx < 8; childIdx++)
{
if (!this->branchHasChild (*node_arg, childIdx))
continue;
const OctreeNode * childNode;
childNode = this->getBranchChildPtr (*node_arg, childIdx);
// generate new key for current branch voxel
OctreeKey newKey;
newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
switch (childNode->getNodeType ())
{
case BRANCH_NODE:
{
// recursively proceed with indexed child branch
voxelCount += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (childNode), newKey, voxelCenterList_arg);
break;
}
case LEAF_NODE:
{
PointT newPoint;
genLeafNodeCenterFromOctreeKey (newKey, newPoint);
voxelCenterList_arg.push_back (newPoint);
voxelCount++;
break;
}
default:
break;
}
}
return (voxelCount);
}
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);
}
