Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <gtest/gtest.h>
00031
00032 #include <pcl_ros/point_cloud.h>
00033 #include <pcl/filters/voxel_grid.h>
00034
00035 #include <pcl18_backports/voxel_grid.h>
00036
00037 #include <mcl_3dl/point_types.h>
00038
00039 TEST(PointTypes, VoxelGrid)
00040 {
00041 pcl::PointCloud<mcl_3dl::PointXYZIL>::Ptr pc(new pcl::PointCloud<mcl_3dl::PointXYZIL>);
00042 pc->resize(3);
00043 pc->points[0].x = 1.0;
00044 pc->points[0].y = 2.0;
00045 pc->points[0].z = 3.0;
00046 pc->points[0].intensity = 4.0;
00047 pc->points[0].label = 1;
00048 pc->points[1].x = 1.02;
00049 pc->points[1].y = 2.02;
00050 pc->points[1].z = 3.02;
00051 pc->points[1].intensity = 5.0;
00052 pc->points[1].label = 3;
00053 pc->points[2].x = -1.0;
00054 pc->points[2].y = -2.0;
00055 pc->points[2].z = -3.0;
00056 pc->points[2].intensity = 6.0;
00057 pc->points[2].label = 4;
00058
00059 pcl::PointCloud<mcl_3dl::PointXYZIL>::Ptr pc2(new pcl::PointCloud<mcl_3dl::PointXYZIL>);
00060 pcl::VoxelGrid18<mcl_3dl::PointXYZIL> ds;
00061 ds.setInputCloud(pc);
00062 ds.setLeafSize(0.1, 0.1, 0.1);
00063 ds.filter(*pc2);
00064
00065 ASSERT_EQ(pc2->size(), 2u);
00066 int num_1(0), num_4(0);
00067 for (auto &p : *pc2)
00068 {
00069 switch (p.label)
00070 {
00071 case 1u:
00072 case 3u:
00073 ASSERT_FLOAT_EQ(p.x, 1.01);
00074 ASSERT_FLOAT_EQ(p.y, 2.01);
00075 ASSERT_FLOAT_EQ(p.z, 3.01);
00076 ASSERT_FLOAT_EQ(p.intensity, 4.5);
00077 num_1++;
00078 break;
00079 case 4u:
00080 ASSERT_EQ(p.x, -1.0);
00081 ASSERT_EQ(p.y, -2.0);
00082 ASSERT_EQ(p.z, -3.0);
00083 ASSERT_EQ(p.intensity, 6.0);
00084 num_4++;
00085 break;
00086 default:
00087 ASSERT_TRUE(false)
00088 << "Unexpected point label (" << p.label << "); "
00089 << "original labels are [1, 3, 4]";
00090 break;
00091 }
00092 }
00093 ASSERT_TRUE(num_1 == 1 && num_4 == 1);
00094 }
00095
00096 int main(int argc, char **argv)
00097 {
00098 testing::InitGoogleTest(&argc, argv);
00099
00100 return RUN_ALL_TESTS();
00101 }