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