From 6817d2278840d27c4279c2e88ce43a60d042b54b Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Fri, 15 Dec 2017 14:45:56 +0100
Subject: [PATCH] Test global 2D SLAM. (#751)

---
 cartographer/mapping/map_builder_test.cc | 52 +++++++++++++++++++++---
 1 file changed, 47 insertions(+), 5 deletions(-)

diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc
index 9c3c994..5525f2e 100644
--- a/cartographer/mapping/map_builder_test.cc
+++ b/cartographer/mapping/map_builder_test.cc
@@ -33,9 +33,9 @@ namespace {
 
 constexpr char kRangeSensorId[] = "range";
 constexpr char kIMUSensorId[] = "imu";
-constexpr double kDuration = 2.;
-constexpr double kTimeStep = 0.1;
-constexpr double kTravelDistance = 0.4;
+constexpr double kDuration = 4.;  // Seconds.
+constexpr double kTimeStep = 0.1;  // Seconds.
+constexpr double kTravelDistance = 1.2;  // Meters.
 
 std::vector<sensor::TimedPointCloudData> GenerateFakeRangeMeasurements() {
   std::vector<sensor::TimedPointCloudData> measurements;
@@ -88,8 +88,8 @@ class MapBuilderTest : public ::testing::Test {
     const std::string kTrajectoryBuilderLua = R"text(
       include "trajectory_builder.lua"
       TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false
-      TRAJECTORY_BUILDER.trajectory_builder_2d.submaps.num_range_data = 5
-      TRAJECTORY_BUILDER.trajectory_builder_3d.submaps.num_range_data = 5
+      TRAJECTORY_BUILDER.trajectory_builder_2d.submaps.num_range_data = 4
+      TRAJECTORY_BUILDER.trajectory_builder_3d.submaps.num_range_data = 4
       return TRAJECTORY_BUILDER)text";
     auto trajectory_builder_parameters =
         ResolveLuaParameters(kTrajectoryBuilderLua);
@@ -106,6 +106,14 @@ class MapBuilderTest : public ::testing::Test {
     map_builder_options_.set_use_trajectory_builder_3d(true);
   }
 
+  void SetOptionsEnableGlobalOptimization() {
+    map_builder_options_.mutable_pose_graph_options()
+        ->set_optimize_every_n_nodes(3);
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->mutable_motion_filter_options()
+        ->set_max_distance_meters(0);
+  }
+
   MapBuilderInterface::LocalSlamResultCallback GetLocalSlamResultCallback() {
     return [=](const int trajectory_id, const ::cartographer::common::Time time,
                const ::cartographer::transform::Rigid3d local_pose,
@@ -200,6 +208,40 @@ TEST_F(MapBuilderTest, LocalSlam3D) {
               0.1 * kTravelDistance);
 }
 
+TEST_F(MapBuilderTest, GlobalSlam2D) {
+  SetOptionsEnableGlobalOptimization();
+  BuildMapBuilder();
+  const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
+  int trajectory_id = map_builder_->AddTrajectoryBuilder(
+      expected_sensor_ids, trajectory_builder_options_,
+      GetLocalSlamResultCallback());
+  TrajectoryBuilderInterface* trajectory_builder =
+      map_builder_->GetTrajectoryBuilder(trajectory_id);
+  const auto measurements = GenerateFakeRangeMeasurements();
+  for (const auto& measurement : measurements) {
+    trajectory_builder->AddSensorData(kRangeSensorId, measurement);
+  }
+  map_builder_->FinishTrajectory(trajectory_id);
+  map_builder_->pose_graph()->RunFinalOptimization();
+  EXPECT_EQ(local_slam_result_poses_.size(), measurements.size());
+  EXPECT_NEAR(kTravelDistance,
+              (local_slam_result_poses_.back().translation() -
+               local_slam_result_poses_.front().translation())
+                  .norm(),
+              0.1 * kTravelDistance);
+  EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50);
+  const auto trajectory_nodes =
+      map_builder_->pose_graph()->GetTrajectoryNodes();
+  EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 20);
+  const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData();
+  EXPECT_GE(submap_data.SizeOfTrajectoryOrZero(trajectory_id), 5);
+  const transform::Rigid3d final_pose =
+      map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id) *
+      local_slam_result_poses_.back();
+  EXPECT_NEAR(kTravelDistance, final_pose.translation().norm(),
+              0.1 * kTravelDistance);
+}
+
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer