Test LoadState and pure localization (#1190)

cartographer/mapping/internal/testing/test_helpers.cc

@@ -41,6 +41,16 @@ ResolveLuaParameters(const std::string& lua_code) {
 std::vector<cartographer::sensor::TimedPointCloudData>
 GenerateFakeRangeMeasurements(double travel_distance, double duration,
                               double time_step) {
+  const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized();
+  return GenerateFakeRangeMeasurements(kDirection * travel_distance, duration,
+                                       time_step,
+                                       transform::Rigid3f::Identity());
+}
+
+std::vector<cartographer::sensor::TimedPointCloudData>
+GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
+                              double duration, double time_step,
+                              const transform::Rigid3f& local_to_global) {
   std::vector<cartographer::sensor::TimedPointCloudData> measurements;
   cartographer::sensor::TimedPointCloud point_cloud;
   for (double angle = 0.; angle < M_PI; angle += 0.01) {
@@ -50,18 +60,18 @@ GenerateFakeRangeMeasurements(double travel_distance, double duration,
                                kRadius * std::sin(angle), height, 0.);
     }
   }
-  const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized();
+  const Eigen::Vector3f kVelocity = translation / duration;
-  const Eigen::Vector3f kVelocity = travel_distance / duration * kDirection;
   for (double elapsed_time = 0.; elapsed_time < duration;
        elapsed_time += time_step) {
     cartographer::common::Time time =
         cartographer::common::FromUniversal(123) +
         cartographer::common::FromSeconds(elapsed_time);
-    cartographer::transform::Rigid3f pose =
+    cartographer::transform::Rigid3f global_pose =
+        local_to_global *
         cartographer::transform::Rigid3f::Translation(elapsed_time * kVelocity);
     cartographer::sensor::TimedPointCloud ranges =
         cartographer::sensor::TransformTimedPointCloud(point_cloud,
-                                                       pose.inverse());
+                                                       global_pose.inverse());
     measurements.emplace_back(cartographer::sensor::TimedPointCloudData{
         time, Eigen::Vector3f::Zero(), ranges});
   }

cartographer/mapping/internal/testing/test_helpers.h

@@ -34,6 +34,11 @@ std::vector<cartographer::sensor::TimedPointCloudData>
 GenerateFakeRangeMeasurements(double travel_distance, double duration,
                               double time_step);
 
+std::vector<cartographer::sensor::TimedPointCloudData>
+GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
+                              double duration, double time_step,
+                              const transform::Rigid3f& local_to_global);
+
 proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1);
 
 proto::Node CreateFakeNode(int trajectory_id = 1, int node_index = 1);

cartographer/mapping/map_builder_test.cc

@@ -17,7 +17,9 @@
 #include "cartographer/mapping/map_builder.h"
 
 #include "cartographer/common/config.h"
+#include "cartographer/io/proto_stream.h"
 #include "cartographer/mapping/internal/testing/test_helpers.h"
+#include "gmock/gmock.h"
 #include "gtest/gtest.h"
 
 namespace cartographer {
@@ -40,6 +42,8 @@ class MapBuilderTest : public ::testing::Test {
       include "map_builder.lua"
       MAP_BUILDER.use_trajectory_builder_2d = true
       MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0
+      MAP_BUILDER.pose_graph.global_sampling_ratio = 0.05
+      MAP_BUILDER.pose_graph.global_constraint_search_after_n_seconds = 0
       return MAP_BUILDER)text";
     auto map_builder_parameters = test::ResolveLuaParameters(kMapBuilderLua);
     map_builder_options_ =
@@ -85,6 +89,21 @@ class MapBuilderTest : public ::testing::Test {
     };
   }
 
+  int CreateTrajectoryWithFakeData() {
+    int trajectory_id = map_builder_->AddTrajectoryBuilder(
+        {kRangeSensorId}, trajectory_builder_options_,
+        GetLocalSlamResultCallback());
+    TrajectoryBuilderInterface* trajectory_builder =
+        map_builder_->GetTrajectoryBuilder(trajectory_id);
+    auto measurements = test::GenerateFakeRangeMeasurements(
+        kTravelDistance, kDuration, kTimeStep);
+    for (auto& measurement : measurements) {
+      trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
+    }
+    map_builder_->FinishTrajectory(trajectory_id);
+    return trajectory_id;
+  }
+
   std::unique_ptr<MapBuilderInterface> map_builder_;
   proto::MapBuilderOptions map_builder_options_;
   proto::TrajectoryBuilderOptions trajectory_builder_options_;
@@ -189,6 +208,10 @@ TEST_F(MapBuilderTest, GlobalSlam2D) {
                   .norm(),
               0.1 * kTravelDistance);
   EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50);
+  EXPECT_THAT(map_builder_->pose_graph()->constraints(),
+              testing::Contains(testing::Field(
+                  &PoseGraphInterface::Constraint::tag,
+                  PoseGraphInterface::Constraint::INTER_SUBMAP)));
   const auto trajectory_nodes =
       map_builder_->pose_graph()->GetTrajectoryNodes();
   EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 20);
@@ -228,6 +251,10 @@ TEST_F(MapBuilderTest, GlobalSlam3D) {
                   .norm(),
               0.1 * kTravelDistance);
   EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 10);
+  EXPECT_THAT(map_builder_->pose_graph()->constraints(),
+              testing::Contains(testing::Field(
+                  &PoseGraphInterface::Constraint::tag,
+                  PoseGraphInterface::Constraint::INTER_SUBMAP)));
   const auto trajectory_nodes =
       map_builder_->pose_graph()->GetTrajectoryNodes();
   EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 5);
@@ -240,6 +267,147 @@ TEST_F(MapBuilderTest, GlobalSlam3D) {
               0.1 * kTravelDistance);
 }
 
+TEST_F(MapBuilderTest, SaveLoadState) {
+  for (int dimensions : {2, 3}) {
+    if (dimensions == 3) {
+      SetOptionsTo3D();
+    }
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->set_use_imu_data(true);
+    BuildMapBuilder();
+    int trajectory_id = map_builder_->AddTrajectoryBuilder(
+        {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_,
+        GetLocalSlamResultCallback());
+    TrajectoryBuilderInterface* trajectory_builder =
+        map_builder_->GetTrajectoryBuilder(trajectory_id);
+    const auto measurements = test::GenerateFakeRangeMeasurements(
+        kTravelDistance, kDuration, kTimeStep);
+    for (const auto& measurement : measurements) {
+      trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
+      trajectory_builder->AddSensorData(
+          kIMUSensorId.id,
+          sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8),
+                          Eigen::Vector3d::Zero()});
+    }
+    map_builder_->FinishTrajectory(trajectory_id);
+    map_builder_->pose_graph()->RunFinalOptimization();
+    int num_constraints = map_builder_->pose_graph()->constraints().size();
+    int num_nodes =
+        map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+            trajectory_id);
+    EXPECT_GT(num_constraints, 0);
+    EXPECT_GT(num_nodes, 0);
+    // TODO(gaschler): Consider using in-memory to avoid side effects.
+    const std::string filename = "temp-SaveLoadState.pbstream";
+    io::ProtoStreamWriter writer(filename);
+    map_builder_->SerializeState(&writer);
+    writer.Close();
+
+    // Reset 'map_builder_'.
+    BuildMapBuilder();
+    io::ProtoStreamReader reader(filename);
+    map_builder_->LoadState(&reader, false /* load_frozen_state */);
+    // TODO(gaschler): Design better way to find out which new trajectory_ids
+    // were created by LoadState.
+    map_builder_->pose_graph()->RunFinalOptimization();
+    auto nodes = map_builder_->pose_graph()->GetTrajectoryNodes();
+    ASSERT_GT(nodes.size(), 0);
+    int new_trajectory_id = nodes.begin()->id.trajectory_id;
+    EXPECT_EQ(num_constraints,
+              map_builder_->pose_graph()->constraints().size());
+    EXPECT_EQ(
+        num_nodes,
+        map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+            new_trajectory_id));
+  }
+}
+
+TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) {
+  BuildMapBuilder();
+  int temp_trajectory_id = CreateTrajectoryWithFakeData();
+  map_builder_->pose_graph()->RunFinalOptimization();
+  EXPECT_GT(map_builder_->pose_graph()->constraints().size(), 0);
+  EXPECT_GT(
+      map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+          temp_trajectory_id),
+      0);
+  const std::string filename = "temp-LocalizationOnFrozenTrajectory2D.pbstream";
+  io::ProtoStreamWriter writer(filename);
+  map_builder_->SerializeState(&writer);
+  writer.Close();
+
+  // Reset 'map_builder_'.
+  local_slam_result_poses_.clear();
+  SetOptionsEnableGlobalOptimization();
+  BuildMapBuilder();
+  io::ProtoStreamReader reader(filename);
+  map_builder_->LoadState(&reader, true /* load_frozen_state */);
+  map_builder_->pose_graph()->RunFinalOptimization();
+  int trajectory_id = map_builder_->AddTrajectoryBuilder(
+      {kRangeSensorId}, trajectory_builder_options_,
+      GetLocalSlamResultCallback());
+  TrajectoryBuilderInterface* trajectory_builder =
+      map_builder_->GetTrajectoryBuilder(trajectory_id);
+  transform::Rigid3d frozen_trajectory_to_global(
+      Eigen::Vector3d(0.5, 0.4, 0),
+      Eigen::Quaterniond(Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitZ())));
+  Eigen::Vector3d travel_translation =
+      Eigen::Vector3d(2., 1., 0.).normalized() * kTravelDistance;
+  auto measurements = test::GenerateFakeRangeMeasurements(
+      travel_translation.cast<float>(), kDuration, kTimeStep,
+      frozen_trajectory_to_global.cast<float>());
+  for (auto& measurement : measurements) {
+    measurement.time += common::FromSeconds(100.);
+    trajectory_builder->AddSensorData(kRangeSensorId.id, 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.15 * kTravelDistance);
+  EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50);
+  auto constraints = map_builder_->pose_graph()->constraints();
+  int num_cross_trajectory_constraints = 0;
+  for (const auto& constraint : constraints) {
+    if (constraint.node_id.trajectory_id !=
+        constraint.submap_id.trajectory_id) {
+      ++num_cross_trajectory_constraints;
+    }
+  }
+  EXPECT_GT(num_cross_trajectory_constraints, 3);
+  // TODO(gaschler): Subscribe global slam callback, verify that all nodes are
+  // optimized.
+  EXPECT_THAT(constraints, testing::Contains(testing::Field(
+                               &PoseGraphInterface::Constraint::tag,
+                               PoseGraphInterface::Constraint::INTER_SUBMAP)));
+  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 global_pose =
+      map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id) *
+      local_slam_result_poses_.back();
+  EXPECT_NEAR(frozen_trajectory_to_global.translation().norm(),
+              map_builder_->pose_graph()
+                  ->GetLocalToGlobalTransform(trajectory_id)
+                  .translation()
+                  .norm(),
+              0.1);
+  const transform::Rigid3d expected_global_pose =
+      frozen_trajectory_to_global *
+      transform::Rigid3d::Translation(travel_translation);
+  EXPECT_NEAR(
+      0.,
+      (global_pose.translation() - expected_global_pose.translation()).norm(),
+      0.3)
+      << "global_pose: " << global_pose
+      << "expected_global_pose: " << expected_global_pose;
+}
+
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer