TEST (PCL, Octree_Pointcloud_Test)
{
size_t i;
int test_runs = 100;
int pointcount = 300;
int test, point;
float resolution = 0.01f;
// instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
OctreePointCloudSearch<PointXYZ> octreeB (resolution);
OctreePointCloudPointVector<PointXYZ> octreeC (resolution);
// create shared pointcloud instances
PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());
// assign input point clouds to octree
octreeA.setInputCloud (cloudA);
octreeB.setInputCloud (cloudB);
octreeC.setInputCloud (cloudB);
for (test = 0; test < test_runs; ++test)
{
// clean up
cloudA->points.clear ();
octreeA.deleteTree ();
cloudB->points.clear ();
octreeB.deleteTree ();
octreeC.deleteTree ();
for (point = 0; point < pointcount; point++)
{
// gereate a random point
PointXYZ newPoint (1024.0 * (float)rand () / (float)RAND_MAX, 1024.0 * (float)rand () / (float)RAND_MAX,
1024.0 * (float)rand () / (float)RAND_MAX);
if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
{
// add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
octreeA.addPointToCloud (newPoint, cloudA);
}
// OctreePointCloudPointVector can store all points..
cloudB->points.push_back (newPoint);
}
ASSERT_EQ(octreeA.getLeafCount(), cloudA->points.size());
// checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
for (i = 0; i < cloudA->points.size (); i++)
{
ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), true);
octreeA.deleteVoxelAtPoint (cloudA->points[i]);
ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), false);
}
ASSERT_EQ(octreeA.getLeafCount(), (unsigned int)0);
// check if all points from leaf data can be found in input pointcloud data sets
octreeB.defineBoundingBox ();
octreeB.addPointsFromInputCloud ();
// test iterator
OctreePointCloudSearch<PointXYZ>::Iterator b_it (octreeB);
unsigned int node_count = 0;
unsigned int branch_count = 0;
unsigned int leaf_count = 0;
double minx, miny, minz, maxx, maxy, maxz;
octreeB.getBoundingBox (minx, miny, minz, maxx, maxy, maxz);
// iterate over tree
while (*++b_it)
{
// depth should always be less than tree depth
unsigned int depth = b_it.getCurrentOctreeDepth ();
ASSERT_LE(depth, octreeB.getTreeDepth());
Eigen::Vector3f voxel_min, voxel_max;
octreeB.getVoxelBounds (b_it, voxel_min, voxel_max);
ASSERT_GE(voxel_min.x (), minx - 1e-4);
ASSERT_GE(voxel_min.y (), miny - 1e-4);
ASSERT_GE(voxel_min.z (), minz - 1e-4);
ASSERT_LE(voxel_max.x (), maxx + 1e-4);
ASSERT_LE(voxel_max.y (), maxy + 1e-4);
ASSERT_LE(voxel_max.z (), maxz + 1e-4);
// store node, branch and leaf count
const OctreeNode* node = b_it.getCurrentOctreeNode ();
if (node->getNodeType () == BRANCH_NODE)
{
branch_count++;
}
else if (node->getNodeType () == LEAF_NODE)
{
leaf_count++;
}
node_count++;
}
// compare node, branch and leaf count against actual tree values
ASSERT_EQ(node_count, branch_count + leaf_count);
ASSERT_EQ(leaf_count, octreeB.getLeafCount ());
for (i = 0; i < cloudB->points.size (); i++)
{
std::vector<int> pointIdxVec;
octreeB.voxelSearch (cloudB->points[i], pointIdxVec);
bool bIdxFound = false;
std::vector<int>::const_iterator current = pointIdxVec.begin ();
while (current != pointIdxVec.end ())
{
if (*current == (int)i)
{
bIdxFound = true;
break;
}
++current;
}
ASSERT_EQ(bIdxFound, true);
}
// test octree pointcloud serialization
std::vector<char> treeBinaryB;
std::vector<char> treeBinaryC;
std::vector<int> leafVectorB;
std::vector<int> leafVectorC;
double minX, minY, minZ, maxX, maxY, maxZ;
// serialize octreeB
octreeB.serializeTree (treeBinaryB, leafVectorB);
octreeB.getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
ASSERT_EQ(cloudB->points.size(), leafVectorB.size());
// deserialize into octreeC
octreeC.deleteTree ();
octreeC.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
octreeC.deserializeTree (treeBinaryB, leafVectorB);
// retrieve point data content of octreeC
octreeC.serializeTree (treeBinaryC, leafVectorC);
// check if data is consistent
ASSERT_EQ(octreeB.getLeafCount(), octreeC.getLeafCount());
ASSERT_EQ(treeBinaryB.size(), treeBinaryC.size());
ASSERT_EQ(octreeB.getLeafCount(), cloudB->points.size());
for (i = 0; i < leafVectorB.size (); ++i)
{
ASSERT_EQ(leafVectorB[i], leafVectorC[i]);
}
// check if binary octree output of both trees is equal
for (i = 0; i < treeBinaryB.size (); ++i)
{
ASSERT_EQ(treeBinaryB[i], treeBinaryC[i]);
}
}
}
TEST (PCL, Octree_Pointcloud_Test)
{
size_t i;
int test_runs = 100;
int pointcount = 300;
int test, point;
float resolution = 0.01f;
// instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
OctreePointCloudPointVector<PointXYZ> octreeB (resolution);
OctreePointCloudPointVector<PointXYZ> octreeC (resolution);
// create shared pointcloud instances
PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());
// assign input point clouds to octree
octreeA.setInputCloud (cloudA);
octreeB.setInputCloud (cloudB);
octreeC.setInputCloud (cloudB);
for (test = 0; test < test_runs; ++test)
{
// clean up
cloudA->points.clear ();
octreeA.deleteTree ();
cloudB->points.clear ();
octreeB.deleteTree ();
octreeC.deleteTree ();
for (point = 0; point < pointcount; point++)
{
// gereate a random point
PointXYZ newPoint (1024.0 * (float)rand () / (float)RAND_MAX, 1024.0 * (float)rand () / (float)RAND_MAX,
1024.0 * (float)rand () / (float)RAND_MAX);
if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
{
// add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
octreeA.addPointToCloud (newPoint, cloudA);
}
// OctreePointCloudPointVector can store all points..
cloudB->points.push_back (newPoint);
}
ASSERT_EQ (octreeA.getLeafCount(), cloudA->points.size());
// checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
for (i = 0; i < cloudA->points.size (); i++)
{
ASSERT_EQ (octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), true);
octreeA.deleteVoxelAtPoint(cloudA->points[i]);
ASSERT_EQ (octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), false);
}
ASSERT_EQ (octreeA.getLeafCount(), 0);
// check if all points from leaf data can be found in input pointcloud data sets
octreeB.defineBoundingBox();
octreeB.addPointsFromInputCloud();
for (i = 0; i < cloudB->points.size (); i++)
{
std::vector<int> pointIdxVec;
octreeB.voxelSearch(cloudB->points[i], pointIdxVec);
bool bIdxFound = false;
std::vector<int>::const_iterator current = pointIdxVec.begin();
while (current != pointIdxVec.end())
{
if (*current == (int)i)
{
bIdxFound = true;
break;
}
++current;
}
ASSERT_EQ (bIdxFound, true);
}
// test octree pointcloud serialization
std::vector<char> treeBinaryB;
std::vector<char> treeBinaryC;
std::vector<int> leafVectorB;
std::vector<int> leafVectorC;
double minX, minY, minZ, maxX, maxY, maxZ;
// serialize octreeB
octreeB.serializeTree(treeBinaryB, leafVectorB);
octreeB.getBoundingBox ( minX, minY,minZ, maxX, maxY, maxZ );
ASSERT_EQ (cloudB->points.size(), leafVectorB.size());
// deserialize into octreeC
octreeC.deleteTree();
octreeC.defineBoundingBox ( minX, minY,minZ, maxX, maxY, maxZ );
octreeC.deserializeTree(treeBinaryB, leafVectorB);
// retrieve point data content of octreeC
octreeC.serializeTree(treeBinaryC, leafVectorC);
// check if data is consistent
ASSERT_EQ (octreeB.getLeafCount(), octreeC.getLeafCount());
ASSERT_EQ (treeBinaryB.size(), treeBinaryC.size() );
ASSERT_EQ (octreeB.getLeafCount(), cloudB->points.size());
for (i = 0; i < leafVectorB.size(); ++i)
{
ASSERT_EQ (leafVectorB[i], leafVectorC[i]);
}
// check if binary octree output of both trees is equal
for (i = 0; i < treeBinaryB.size(); ++i)
{
ASSERT_EQ (treeBinaryB[i], treeBinaryC[i]);
}
}
}
TEST (PCL, Octree_Pointcloud_Test)
{
size_t i;
int test_runs = 100;
int pointcount = 300;
int test, point;
float resolution = 0.01f;
// instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
OctreePointCloudSearch<PointXYZ> octreeB (resolution);
OctreePointCloudPointVector<PointXYZ> octreeC (resolution);
// create shared pointcloud instances
PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());
// assign input point clouds to octree
octreeA.setInputCloud (cloudA);
octreeB.setInputCloud (cloudB);
octreeC.setInputCloud (cloudB);
for (test = 0; test < test_runs; ++test)
{
// clean up
cloudA->points.clear ();
octreeA.deleteTree ();
cloudB->points.clear ();
octreeB.deleteTree ();
octreeC.deleteTree ();
for (point = 0; point < pointcount; point++)
{
// gereate a random point
PointXYZ newPoint (static_cast<float> (1024.0 * rand () / RAND_MAX),
static_cast<float> (1024.0 * rand () / RAND_MAX),
static_cast<float> (1024.0 * rand () / RAND_MAX));
if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
{
// add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
octreeA.addPointToCloud (newPoint, cloudA);
}
// OctreePointCloudPointVector can store all points..
cloudB->push_back (newPoint);
}
ASSERT_EQ (cloudA->points.size (), octreeA.getLeafCount ());
// checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
for (i = 0; i < cloudA->points.size (); i++)
{
ASSERT_TRUE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i]));
octreeA.deleteVoxelAtPoint (cloudA->points[i]);
ASSERT_FALSE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i]));
}
ASSERT_EQ (static_cast<unsigned int> (0), octreeA.getLeafCount ());
// check if all points from leaf data can be found in input pointcloud data sets
octreeB.defineBoundingBox ();
octreeB.addPointsFromInputCloud ();
// test iterator
OctreePointCloudPointVector<PointXYZ>::Iterator b_it;
OctreePointCloudPointVector<PointXYZ>::Iterator b_it_end = octreeB.end();
// iterate over tree
unsigned int node_count = 0;
unsigned int branch_count = 0;
unsigned int leaf_count = 0;
double minx, miny, minz, maxx, maxy, maxz;
octreeB.getBoundingBox (minx, miny, minz, maxx, maxy, maxz);
// iterate over tree
for (b_it = octreeB.begin(); b_it != b_it_end; ++b_it)
{
// depth should always be less than tree depth
unsigned int depth = b_it.getCurrentOctreeDepth ();
ASSERT_LE (depth, octreeB.getTreeDepth ());
Eigen::Vector3f voxel_min, voxel_max;
octreeB.getVoxelBounds (b_it, voxel_min, voxel_max);
ASSERT_GE (voxel_min.x (), minx - 1e-4);
ASSERT_GE (voxel_min.y (), miny - 1e-4);
ASSERT_GE (voxel_min.z (), minz - 1e-4);
ASSERT_LE (voxel_max.x (), maxx + 1e-4);
ASSERT_LE (voxel_max.y (), maxy + 1e-4);
ASSERT_LE (voxel_max.z (), maxz + 1e-4);
// store node, branch and leaf count
const OctreeNode* node = b_it.getCurrentOctreeNode ();
if (node->getNodeType () == BRANCH_NODE)
{
branch_count++;
}
else if (node->getNodeType () == LEAF_NODE)
{
leaf_count++;
}
node_count++;
}
// compare node, branch and leaf count against actual tree values
ASSERT_EQ (branch_count + leaf_count, node_count);
ASSERT_EQ (octreeB.getLeafCount (), leaf_count);
for (i = 0; i < cloudB->points.size (); i++)
{
std::vector<int> pointIdxVec;
octreeB.voxelSearch (cloudB->points[i], pointIdxVec);
bool bIdxFound = false;
std::vector<int>::const_iterator current = pointIdxVec.begin ();
while (current != pointIdxVec.end ())
{
if (*current == static_cast<int> (i))
{
bIdxFound = true;
break;
}
++current;
}
ASSERT_TRUE (bIdxFound);
}
}
}