Allow introduction of new landmarks into loaded (frozen) map (#1475)

In the current implementation, as soon as one trajectory is frozen it is not possible to use new landmarks anymore.
In this PR a suggest a change, that allows introducing new landmarks also in the presence of a frozen trajectory. This is done with the following steps:
- Store set of frozen landmarks
- Landmarks that are loaded from a pbstream, with the flag ` load_frozen_state=true` are inserted into the set of frozen landmarks via `setLandmarkPose` function
- frozen landmarks are set constant in the optimization problem
- new landmarks can be introduced and will be optimized in the optimization problem

Motivated by and closes #1470

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -138,7 +138,8 @@ std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses()
 }
 
 void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id,
-                                    const transform::Rigid3d& global_pose) {
+                                    const transform::Rigid3d& global_pose,
+                                    const bool frozen) {
   proto::SetLandmarkPoseRequest request;
   request.mutable_landmark_pose()->set_landmark_id(landmark_id);
   *request.mutable_landmark_pose()->mutable_global_pose() =

cartographer/cloud/internal/client/pose_graph_stub.h

@@ -45,7 +45,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
   std::map<int, TrajectoryState> GetTrajectoryStates() const override;
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override;
   void SetLandmarkPose(const std::string& landmark_id,
-                       const transform::Rigid3d& global_pose) override;
+                       const transform::Rigid3d& global_pose,
+                       const bool frozen = false) override;
   void DeleteTrajectory(int trajectory_id) override;
   bool IsTrajectoryFinished(int trajectory_id) const override;
   bool IsTrajectoryFrozen(int trajectory_id) const override;

cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc

@@ -53,7 +53,8 @@ TEST_F(SetLandmarkPoseHandlerTest, SetLandmarkPose) {
                       transform::IsNearly(
                           transform::Rigid3d(Eigen::Vector3d(1, 2, 3),
                                              Eigen::Quaterniond(4, 5, 6, 7)),
-                          kEps)));
+                          kEps),
+                      false));
   test_server_->SendWrite(request);
 }
 

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -927,10 +927,12 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
 }
 
 void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
-                                  const transform::Rigid3d& global_pose) {
+                                  const transform::Rigid3d& global_pose,
+                                  const bool frozen) {
   AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
     absl::MutexLock locker(&mutex_);
     data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
+    data_.landmark_nodes[landmark_id].frozen = frozen;
     return WorkItem::Result::kDoNotRunOptimization;
   });
 }

cartographer/mapping/internal/2d/pose_graph_2d.h

@@ -133,7 +133,8 @@ class PoseGraph2D : public PoseGraph {
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
       LOCKS_EXCLUDED(mutex_);
   void SetLandmarkPose(const std::string& landmark_id,
-                       const transform::Rigid3d& global_pose) override
+                       const transform::Rigid3d& global_pose,
+                       const bool frozen = false) override
       LOCKS_EXCLUDED(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() const override
       LOCKS_EXCLUDED(mutex_);

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -948,10 +948,12 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
 }
 
 void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id,
-                                  const transform::Rigid3d& global_pose) {
+                                  const transform::Rigid3d& global_pose,
+                                  const bool frozen) {
   AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
     absl::MutexLock locker(&mutex_);
     data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
+    data_.landmark_nodes[landmark_id].frozen = frozen;
     return WorkItem::Result::kDoNotRunOptimization;
   });
 }

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -131,7 +131,8 @@ class PoseGraph3D : public PoseGraph {
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
       LOCKS_EXCLUDED(mutex_);
   void SetLandmarkPose(const std::string& landmark_id,
-                       const transform::Rigid3d& global_pose) override
+                       const transform::Rigid3d& global_pose,
+                       const bool frozen = false) override
       LOCKS_EXCLUDED(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() const override
       LOCKS_EXCLUDED(mutex_);

cartographer/mapping/internal/optimization/optimization_problem_2d.cc

@@ -79,16 +79,11 @@ transform::Rigid3d GetInitialLandmarkPose(
 
 void AddLandmarkCostFunctions(
     const std::map<std::string, LandmarkNode>& landmark_nodes,
-    bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
+    const MapById<NodeId, NodeSpec2D>& node_data,
     MapById<NodeId, std::array<double, 3>>* C_nodes,
     std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
     double huber_scale) {
   for (const auto& landmark_node : landmark_nodes) {
-    // Do not use landmarks that were not optimized for localization.
-    if (!landmark_node.second.global_landmark_pose.has_value() &&
-        freeze_landmarks) {
-      continue;
-    }
     for (const auto& observation : landmark_node.second.landmark_observations) {
       const std::string& landmark_id = landmark_node.first;
       const auto& begin_of_trajectory =
@@ -123,7 +118,8 @@ void AddLandmarkCostFunctions(
             CeresPose(starting_point, nullptr /* translation_parametrization */,
                       absl::make_unique<ceres::QuaternionParameterization>(),
                       problem));
-        if (freeze_landmarks) {
+        // Set landmark constant if it is frozen.
+        if (landmark_node.second.frozen) {
           problem->SetParameterBlockConstant(
               C_landmarks->at(landmark_id).translation());
           problem->SetParameterBlockConstant(
@@ -220,7 +216,6 @@ void OptimizationProblem2D::Solve(
   MapById<NodeId, std::array<double, 3>> C_nodes;
   std::map<std::string, CeresPose> C_landmarks;
   bool first_submap = true;
-  bool freeze_landmarks = !frozen_trajectories.empty();
   for (const auto& submap_id_data : submap_data_) {
     const bool frozen =
         frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
@@ -255,9 +250,8 @@ void OptimizationProblem2D::Solve(
         C_nodes.at(constraint.node_id).data());
   }
   // Add cost functions for landmarks.
-  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
+  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
-                           &C_nodes, &C_landmarks, &problem,
+                           &problem, options_.huber_scale());
-                           options_.huber_scale());
   // Add penalties for violating odometry or changes between consecutive nodes
   // if odometry is not available.
   for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {

cartographer/mapping/internal/optimization/optimization_problem_3d.cc

@@ -123,16 +123,12 @@ transform::Rigid3d GetInitialLandmarkPose(
 
 void AddLandmarkCostFunctions(
     const std::map<std::string, LandmarkNode>& landmark_nodes,
-    bool freeze_landmarks, const MapById<NodeId, NodeSpec3D>& node_data,
+    const MapById<NodeId, NodeSpec3D>& node_data,
     MapById<NodeId, CeresPose>* C_nodes,
     std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
     double huber_scale) {
   for (const auto& landmark_node : landmark_nodes) {
     // Do not use landmarks that were not optimized for localization.
-    if (!landmark_node.second.global_landmark_pose.has_value() &&
-        freeze_landmarks) {
-      continue;
-    }
     for (const auto& observation : landmark_node.second.landmark_observations) {
       const std::string& landmark_id = landmark_node.first;
       const auto& begin_of_trajectory =
@@ -167,7 +163,8 @@ void AddLandmarkCostFunctions(
             CeresPose(starting_point, nullptr /* translation_parametrization */,
                       absl::make_unique<ceres::QuaternionParameterization>(),
                       problem));
-        if (freeze_landmarks) {
+        // Set landmark constant if it is frozen.
+        if (landmark_node.second.frozen) {
           problem->SetParameterBlockConstant(
               C_landmarks->at(landmark_id).translation());
           problem->SetParameterBlockConstant(
@@ -291,7 +288,6 @@ void OptimizationProblem3D::Solve(
   MapById<NodeId, CeresPose> C_nodes;
   std::map<std::string, CeresPose> C_landmarks;
   bool first_submap = true;
-  bool freeze_landmarks = !frozen_trajectories.empty();
   for (const auto& submap_id_data : submap_data_) {
     const bool frozen =
         frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
@@ -351,9 +347,8 @@ void OptimizationProblem3D::Solve(
         C_nodes.at(constraint.node_id).translation());
   }
   // Add cost functions for landmarks.
-  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
+  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
-                           &C_nodes, &C_landmarks, &problem,
+                           &problem, options_.huber_scale());
-                           options_.huber_scale());
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
   if (!options_.fix_z_in_3d()) {

cartographer/mapping/internal/testing/mock_pose_graph.h

@@ -48,8 +48,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
       std::map<int, mapping::PoseGraphInterface::TrajectoryState>());
   MOCK_CONST_METHOD0(GetLandmarkPoses,
                      std::map<std::string, transform::Rigid3d>());
-  MOCK_METHOD2(SetLandmarkPose,
+  MOCK_METHOD3(SetLandmarkPose,
-               void(const std::string&, const transform::Rigid3d&));
+               void(const std::string&, const transform::Rigid3d&, const bool));
   MOCK_METHOD1(DeleteTrajectory, void(int));
   MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int));
   MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int));

cartographer/mapping/map_builder.cc

@@ -279,7 +279,8 @@ std::map<int, int> MapBuilder::LoadState(
   // Set global poses of landmarks.
   for (const auto& landmark : pose_graph_proto.landmark_poses()) {
     pose_graph_->SetLandmarkPose(landmark.landmark_id(),
-                                 transform::ToRigid3(landmark.global_pose()));
+                                 transform::ToRigid3(landmark.global_pose()),
+                                 true);
   }
 
   MapById<SubmapId, mapping::proto::Submap> submap_id_to_submap;

cartographer/mapping/pose_graph_interface.h

@@ -62,6 +62,7 @@ class PoseGraphInterface {
     };
     std::vector<LandmarkObservation> landmark_observations;
     absl::optional<transform::Rigid3d> global_landmark_pose;
+    bool frozen = false;
   };
 
   struct SubmapPose {
@@ -123,7 +124,8 @@ class PoseGraphInterface {
 
   // Sets global pose of landmark 'landmark_id' to given 'global_pose'.
   virtual void SetLandmarkPose(const std::string& landmark_id,
-                               const transform::Rigid3d& global_pose) = 0;
+                               const transform::Rigid3d& global_pose,
+                               const bool frozen = false) = 0;
 
   // Deletes a trajectory asynchronously.
   virtual void DeleteTrajectory(int trajectory_id) = 0;