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 ())); }
void point_test(octree_disk& t) { boost::mt19937 rng(rngseed); boost::uniform_real<float> dist(0,1); double query_box_min[3]; double qboxmax[3]; for(int i = 0; i < 10; i++) { //std::cout << "query test round " << i << std::endl; for(int j = 0; j < 3; j++) { query_box_min[j] = dist(rng); qboxmax[j] = dist(rng); if(qboxmax[j] < query_box_min[j]) { std::swap(query_box_min[j], qboxmax[j]); } } //query the trees AlignedPointTVector p_ot; t.queryBBIncludes(query_box_min, qboxmax, t.getDepth(), p_ot); //query the list AlignedPointTVector pointsinregion; for(AlignedPointTVector::iterator pointit = points.begin (); pointit != points.end (); ++pointit) { if((query_box_min[0] <= pointit->x) && (pointit->x < qboxmax[0]) && (query_box_min[1] < pointit->y) && (pointit->y < qboxmax[1]) && (query_box_min[2] <= pointit->z) && (pointit->z < qboxmax[2])) { pointsinregion.push_back(*pointit); } } EXPECT_EQ (p_ot.size (), pointsinregion.size ()); //very slow exhaustive comparison while( !p_ot.empty () ) { AlignedPointTVector::iterator it; it = std::find_first_of(p_ot.begin(), p_ot.end(), pointsinregion.begin (), pointsinregion.end (), compPt); if(it != p_ot.end()) { p_ot.erase(it); } else { FAIL () << "Dropped Point from tree1!" << std::endl; break; } } EXPECT_TRUE(p_ot.empty()); } }
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> void OutofcoreOctreeRamContainer<PointT>::readRangeSubSample (const boost::uint64_t start, const boost::uint64_t count, const double percent, AlignedPointTVector& v) { /** \todo change the subsampling technique to use built in PCL sampling */ boost::uint64_t samplesize = static_cast<boost::uint64_t> (percent * static_cast<double> (count)); boost::mutex::scoped_lock lock (rng_mutex_); boost::uniform_int < boost::uint64_t > buffdist (start, start + count); boost::variate_generator<boost::mt19937&, boost::uniform_int<boost::uint64_t> > buffdie (rand_gen_, buffdist); for (boost::uint64_t i = 0; i < samplesize; i++) { boost::uint64_t buffstart = buffdie (); v.push_back (container_[buffstart]); } }
TEST (PCL, Outofcore_Ram_Tree) { Eigen::Vector3d min (0.0,0.0,0.0); Eigen::Vector3d max (1.0, 1.0, 1.0); const boost::filesystem::path filename_otreeA = "ram_tree/ram_tree.oct_idx"; octree_ram t (min, max, .1, filename_otreeA, "ECEF"); boost::mt19937 rng (rngseed); //boost::uniform_real<double> dist(0,1);//for testing sparse boost::normal_distribution<float> dist (0.5f, .1f);//for testing less sparse PointT p; points.resize (numPts); for (size_t i = 0; i < numPts; i++) { p.x = dist(rng); p.y = dist(rng); p.z = dist(rng); points[i] = p; } t.addDataToLeaf_and_genLOD (points); //t.addDataToLeaf(points); Eigen::Vector3d qboxmin; Eigen::Vector3d qboxmax; for (int i = 0; i < 10; i++) { //std::cout << "query test round " << i << std::endl; for (int j = 0; j < 3; j++) { qboxmin[j] = dist (rng); qboxmax[j] = dist (rng); if (qboxmax[j] < qboxmin[j]) { std::swap (qboxmin[j], qboxmax[j]); } } //query the trees AlignedPointTVector p_ot1; t.queryBBIncludes (qboxmin, qboxmax, t.getDepth (), p_ot1); //query the list AlignedPointTVector pointsinregion; BOOST_FOREACH(const PointT& p, points) { if ((qboxmin[0] <= p.x) && (p.x <= qboxmax[0]) && (qboxmin[1] <= p.y) && (p.y <= qboxmax[1]) && (qboxmin[2] <= p.z) && (p.z <= qboxmax[2])) { pointsinregion.push_back (p); } } EXPECT_EQ (p_ot1.size (), pointsinregion.size ()); //very slow exhaustive comparison while (!p_ot1.empty ()) { AlignedPointTVector::iterator it; it = std::find_first_of (p_ot1.begin (), p_ot1.end (), pointsinregion.begin (), pointsinregion.end (), compPt); if (it != p_ot1.end ()) { p_ot1.erase(it); } else { break; FAIL () << "Dropped Point from tree1!" << std::endl; } } EXPECT_TRUE (p_ot1.empty ()); } }
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); }