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); }