Fix tests

Before the origin of the RangefinderPoint was not always initialized. This generate instable test results. This commit fixes this problem by initializing properly the origin to (0,0,0).
cartographer-project · Apr 3, 2022 · fdf9e53 · fdf9e53
1 parent 07c7d28
commit fdf9e53
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 22 deletions.
diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc
@@ -150,12 +150,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
   const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
 
   sensor::PointCloud point_cloud;
-  point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
-  point_cloud.push_back({Eigen::Vector3f{-2.f, 0.5f, 0.f}});
-  point_cloud.push_back({Eigen::Vector3f{0.f, -0.5f, 0.f}});
-  point_cloud.push_back({Eigen::Vector3f{0.5f, -1.6f, 0.f}});
-  point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
-  point_cloud.push_back({Eigen::Vector3f{2.5f, 1.7f, 0.f}});
+  Eigen::Vector3f origin(0.f, 0.f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}, origin});
+  point_cloud.push_back({Eigen::Vector3f{-2.f, 0.5f, 0.f}, origin});
+  point_cloud.push_back({Eigen::Vector3f{0.f, -0.5f, 0.f}, origin});
+  point_cloud.push_back({Eigen::Vector3f{0.5f, -1.6f, 0.f}, origin});
+  point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}, origin});
+  point_cloud.push_back({Eigen::Vector3f{2.5f, 1.7f, 0.f}, origin});
 
   for (int i = 0; i != 50; ++i) {
     const transform::Rigid2f expected_pose(
@@ -200,12 +201,16 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
   const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);
 
   sensor::PointCloud unperturbed_point_cloud;
-  unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
-  unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.25f, 0.5f, 0.f}});
-  unperturbed_point_cloud.push_back({Eigen::Vector3f{0.f, 0.5f, 0.f}});
-  unperturbed_point_cloud.push_back({Eigen::Vector3f{0.25f, 1.6f, 0.f}});
-  unperturbed_point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
-  unperturbed_point_cloud.push_back({Eigen::Vector3f{2.f, 1.8f, 0.f}});
+  Eigen::Vector3f origin(0.f, 0.f, 0.f);
+  unperturbed_point_cloud.push_back(
+      {Eigen::Vector3f{-2.5f, 0.5f, 0.f}, origin});
+  unperturbed_point_cloud.push_back(
+      {Eigen::Vector3f{-2.25f, 0.5f, 0.f}, origin});
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{0.f, 0.5f, 0.f}, origin});
+  unperturbed_point_cloud.push_back(
+      {Eigen::Vector3f{0.25f, 1.6f, 0.f}, origin});
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}, origin});
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{2.f, 1.8f, 0.f}, origin});
 
   for (int i = 0; i != 20; ++i) {
     const transform::Rigid2f perturbation(

diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc
@@ -55,33 +55,39 @@ TEST(PointCloudTest, TransformTimedPointCloud) {
 }
 
 TEST(PointCloudTest, CopyIf) {
-  std::vector<RangefinderPoint> points = {
-      {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}};
+  Eigen::Vector3f origin(0.f, 0.f, 0.f);
+  std::vector<RangefinderPoint> points = {{{0.f, 0.f, 0.f}, origin},
+                                          {{1.f, 1.f, 1.f}, origin},
+                                          {{2.f, 2.f, 2.f}, origin}};
 
   const PointCloud point_cloud(points);
   const PointCloud copied_point_cloud = point_cloud.copy_if(
       [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; });
 
   EXPECT_EQ(copied_point_cloud.size(), 2);
   EXPECT_THAT(copied_point_cloud.intensities(), IsEmpty());
-  EXPECT_THAT(copied_point_cloud.points(),
-              ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}},
-                          RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}}));
+  EXPECT_THAT(
+      copied_point_cloud.points(),
+      ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}, origin},
+                  RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}, origin}));
 }
 
 TEST(PointCloudTest, CopyIfWithIntensities) {
-  std::vector<RangefinderPoint> points = {
-      {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}};
+  Eigen::Vector3f origin(0.f, 0.f, 0.f);
+  std::vector<RangefinderPoint> points = {{{0.f, 0.f, 0.f}, origin},
+                                          {{1.f, 1.f, 1.f}, origin},
+                                          {{2.f, 2.f, 2.f}, origin}};
   std::vector<float> intensities = {0.f, 1.f, 2.f};
 
   const PointCloud point_cloud(points, intensities);
   const PointCloud copied_point_cloud = point_cloud.copy_if(
       [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; });
   EXPECT_EQ(copied_point_cloud.size(), 2);
   EXPECT_EQ(copied_point_cloud.intensities().size(), 2);
-  EXPECT_THAT(copied_point_cloud.points(),
-              ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}},
-                          RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}}));
+  EXPECT_THAT(
+      copied_point_cloud.points(),
+      ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}, origin},
+                  RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}, origin}));
   EXPECT_THAT(copied_point_cloud.intensities(),
               ElementsAre(FloatNear(1.f, 1e-6), FloatNear(2.f, 1e-6)));
 }