TEST (PCL, Octree_Pointcloud_Voxel_Centroid_Test)
{
// instantiate point cloud
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
OctreePointCloudVoxelCentroid<PointXYZ> octree (1.0f);
octree.defineBoundingBox(10.0, 10.0, 10.0);
size_t i;
srand (time (NULL));
cloudIn->width = 10*3;
cloudIn->height = 1;
cloudIn->points.resize (cloudIn->width * cloudIn->height);
// generate point data for point cloud
for (i = 0; i < 10; i++)
{
// these three points should always be assigned to the same voxel in the octree
cloudIn->points[i*3+0] = PointXYZ ( (float)i+0.2, (float)i+0.2, (float)i+0.2 );
cloudIn->points[i*3+1] = PointXYZ ( (float)i+0.4, (float)i+0.4, (float)i+0.4 );
cloudIn->points[i*3+2] = PointXYZ ( (float)i+0.6, (float)i+0.6, (float)i+0.6 );
}
// assign point cloud to octree
octree.setInputCloud (cloudIn);
// add points from cloud to octree
octree.addPointsFromInputCloud ();
std::vector<PointXYZ, Eigen::aligned_allocator<PointXYZ> > voxelCentroids;
octree.getVoxelCentroids (voxelCentroids);
// we expect 10 voxel centroids
ASSERT_EQ ( voxelCentroids.size() , 10 );
// check centroid calculation
for (i = 0; i < 10; i++)
{
EXPECT_NEAR (voxelCentroids[i].x, (float)i+0.4, 1e-4);
EXPECT_NEAR (voxelCentroids[i].y, (float)i+0.4, 1e-4);
EXPECT_NEAR (voxelCentroids[i].z, (float)i+0.4, 1e-4);
}
}
TEST (PCL, Octree_Pointcloud_Voxel_Centroid_Test)
{
// instantiate point cloud
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
OctreePointCloudVoxelCentroid<PointXYZ> octree (1.0f);
octree.defineBoundingBox (10.0, 10.0, 10.0);
size_t i;
srand (static_cast<unsigned int> (time (NULL)));
cloudIn->width = 10 * 3;
cloudIn->height = 1;
cloudIn->points.resize (cloudIn->width * cloudIn->height);
// generate point data for point cloud
for (i = 0; i < 10; i++)
{
// these three points should always be assigned to the same voxel in the octree
cloudIn->points[i * 3 + 0] = PointXYZ (static_cast<float> (i) + 0.2f, static_cast<float> (i) + 0.2f, static_cast<float> (i) + 0.2f);
cloudIn->points[i * 3 + 1] = PointXYZ (static_cast<float> (i) + 0.4f, static_cast<float> (i) + 0.4f, static_cast<float> (i) + 0.4f);
cloudIn->points[i * 3 + 2] = PointXYZ (static_cast<float> (i) + 0.6f, static_cast<float> (i) + 0.6f, static_cast<float> (i) + 0.6f);
}
// assign point cloud to octree
octree.setInputCloud (cloudIn);
// add points from cloud to octree
octree.addPointsFromInputCloud ();
pcl::PointCloud<PointXYZ>::VectorType voxelCentroids;
octree.getVoxelCentroids (voxelCentroids);
// we expect 10 voxel centroids
ASSERT_EQ (static_cast<std::size_t> (10), voxelCentroids.size());
// check centroid calculation
for (i = 0; i < 10; i++)
{
EXPECT_NEAR (voxelCentroids[i].x, static_cast<float> (i) + 0.4, 1e-4);
EXPECT_NEAR (voxelCentroids[i].y, static_cast<float> (i) + 0.4, 1e-4);
EXPECT_NEAR (voxelCentroids[i].z, static_cast<float> (i) + 0.4, 1e-4);
}
// ADDING AN ADDITIONAL POINT CLOUD TO OctreePointCloudVoxelCentroid
// generate new point data
for (i = 0; i < 10; i++)
{
// these three points should always be assigned to the same voxel in the octree
cloudIn->points[i * 3 + 0] = PointXYZ (static_cast<float> (i) + 0.1f, static_cast<float> (i) + 0.1f, static_cast<float> (i) + 0.1f);
cloudIn->points[i * 3 + 1] = PointXYZ (static_cast<float> (i) + 0.4f, static_cast<float> (i) + 0.4f, static_cast<float> (i) + 0.4f);
cloudIn->points[i * 3 + 2] = PointXYZ (static_cast<float> (i) + 0.7f, static_cast<float> (i) + 0.7f, static_cast<float> (i) + 0.7f);
}
// add points from new cloud to octree
octree.setInputCloud (cloudIn);
octree.addPointsFromInputCloud ();
voxelCentroids.clear();
octree.getVoxelCentroids (voxelCentroids);
// check centroid calculation
for (i = 0; i < 10; i++)
{
EXPECT_NEAR (voxelCentroids[i].x, static_cast<float> (i) + 0.4, 1e-4);
EXPECT_NEAR (voxelCentroids[i].y, static_cast<float> (i) + 0.4, 1e-4);
EXPECT_NEAR (voxelCentroids[i].z, static_cast<float> (i) + 0.4, 1e-4);
}
}
