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>::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 Container, typename PointT> boost::uint64_t
octree_base<Container, PointT>::addDataToLeaf (const AlignedPointTVector& p)
{
boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
const bool _FORCE_BB_CHECK = true;
uint64_t pt_added = root_->addDataToLeaf (p, _FORCE_BB_CHECK);
assert (p.size () == pt_added);
return (pt_added);
}
//loads chunks of up to 2e9 pts at a time; this is a completely arbitrary number
template<typename Container, typename PointT> void
octree_base<Container, PointT>::buildLOD (octree_base_node<Container, PointT>** current_branch, const int current_dims)
{
//stop if this brach DNE
if (!current_branch[current_dims - 1])
{
return;
}
if ((current_branch[current_dims - 1]->numchildren () == 0)
&& (!current_branch[current_dims - 1]->hasUnloadedChildren ()))//at leaf: subsample, remove, and copy to higher nodes
{
//this node's idx is (current_dims-1)
octree_base_node<Container, PointT>* leaf = current_branch[current_dims - 1];
boost::uint64_t leaf_start_size = leaf->payload->size ();
if (leaf_start_size > 0)//skip empty
{
for (boost::uint64_t startp = 0; startp < leaf_start_size; startp += LOAD_COUNT_)
{
//there are (current_dims-1) nodes above this one, indexed 0 thru (current_dims-2)
for (size_t level = (current_dims - 1); level >= 1; level--)
{
//the target
octree_base_node<Container, PointT>* target_parent = current_branch[level - 1];
//the percent to copy
//each level up the chain gets sample_precent^l of the leaf's data
double percent = pow (double (octree_base_node<Container, PointT>::sample_precent), double (current_dims - level));
//read in percent of node
AlignedPointTVector v;
if ((startp + LOAD_COUNT_) < leaf_start_size)
{
leaf->payload->readRangeSubSample (startp, LOAD_COUNT_, percent, v);
}
else
{
leaf->payload->readRangeSubSample (startp, leaf_start_size - startp, percent, v);
}
//write to the target
if (!v.empty ())
{
target_parent->payload->insertRange ( v );
// target_parent->payload->insertRange (&(v.front ()), v.size ());
this->incrementPointsInLOD (target_parent->depth, v.size ());
}
}
}
}
}
else//not at leaf, keep going down
{
//clear this node, in case we are updating the LOD
current_branch[current_dims - 1]->payload->clear ();
const int next_dims = current_dims + 1;
octree_base_node<Container, PointT>** next_branch = new octree_base_node<Container, PointT>*[next_dims];
memcpy (next_branch, current_branch, current_dims * sizeof(octree_base_node<Container, PointT>**));
size_t numchild = current_branch[current_dims - 1]->numchildren ();
if ((numchild != 8) && (current_branch[current_dims - 1]->hasUnloadedChildren ()))
{
current_branch[current_dims - 1]->loadChildren (false);
numchild = current_branch[current_dims - 1]->numchildren ();
}
for (size_t i = 0; i < numchild; i++)
{
next_branch[next_dims - 1] = next_branch[current_dims - 1]->children[i];
buildLOD (next_branch, next_dims);
}
delete[] next_branch;
}
}
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 ());
}
}
