30 #include <gtest/gtest.h> 33 #include <pcl/filters/voxel_grid.h> 41 pcl::PointCloud<mcl_3dl::PointXYZIL>::Ptr pc(
new pcl::PointCloud<mcl_3dl::PointXYZIL>);
43 pc->points[0].x = 1.0;
44 pc->points[0].y = 2.0;
45 pc->points[0].z = 3.0;
46 pc->points[0].intensity = 4.0;
47 pc->points[0].label = 1;
48 pc->points[1].x = 1.02;
49 pc->points[1].y = 2.02;
50 pc->points[1].z = 3.02;
51 pc->points[1].intensity = 5.0;
52 pc->points[1].label = 3;
53 pc->points[2].x = -1.0;
54 pc->points[2].y = -2.0;
55 pc->points[2].z = -3.0;
56 pc->points[2].intensity = 6.0;
57 pc->points[2].label = 4;
59 pcl::PointCloud<mcl_3dl::PointXYZIL>::Ptr pc2(
new pcl::PointCloud<mcl_3dl::PointXYZIL>);
62 ds.setLeafSize(0.1, 0.1, 0.1);
65 ASSERT_EQ(pc2->size(), 2u);
66 int num_1(0), num_4(0);
73 ASSERT_FLOAT_EQ(p.x, 1.01);
74 ASSERT_FLOAT_EQ(p.y, 2.01);
75 ASSERT_FLOAT_EQ(p.z, 3.01);
76 ASSERT_FLOAT_EQ(p.intensity, 4.5);
83 ASSERT_EQ(p.intensity, 6.0);
88 <<
"Unexpected point label (" << p.label <<
"); " 89 <<
"original labels are [1, 3, 4]";
93 ASSERT_TRUE(num_1 == 1 && num_4 == 1);
96 int main(
int argc,
char **argv)
98 testing::InitGoogleTest(&argc, argv);
100 return RUN_ALL_TESTS();
virtual void filter(const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output)
TEST(PointTypes, VoxelGrid)
int main(int argc, char **argv)