Add landmarks to the state and deserialize them. (#988)

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc

@@ -74,10 +74,15 @@ transform::Rigid3d GetInitialLandmarkPose(
 
 void AddLandmarkCostFunctions(
     const std::map<std::string, LandmarkNode>& landmark_nodes,
-    const MapById<NodeId, NodeData>& node_data,
+    bool freeze_landmarks, const MapById<NodeId, NodeData>& node_data,
     MapById<NodeId, std::array<double, 3>>* C_nodes,
     std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
   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 =
@@ -109,6 +114,12 @@ void AddLandmarkCostFunctions(
             CeresPose(starting_point, nullptr /* translation_parametrization */,
                       common::make_unique<ceres::QuaternionParameterization>(),
                       problem));
+        if (freeze_landmarks) {
+          problem->SetParameterBlockConstant(
+              C_landmarks->at(landmark_id).translation());
+          problem->SetParameterBlockConstant(
+              C_landmarks->at(landmark_id).rotation());
+        }
       }
       problem->AddResidualBlock(
           LandmarkCostFunction2D::CreateAutoDiffCostFunction(
@@ -198,6 +209,7 @@ 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;
@@ -232,8 +244,8 @@ void OptimizationProblem2D::Solve(
         C_nodes.at(constraint.node_id).data());
   }
   // Add cost functions for landmarks.
-  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
+  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
-                           &problem);
+                           &C_nodes, &C_landmarks, &problem);
   // 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/2d/pose_graph_2d.cc

@@ -550,8 +550,8 @@ void PoseGraph2D::RunOptimization() {
   }
 
   // No other thread is accessing the optimization_problem_, constraints_,
-  // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
+  // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
-  // is time consuming, so not taking the mutex before Solve to avoid blocking
+  // time consuming, so not taking the mutex before Solve to avoid blocking
   // foreground processing.
   optimization_problem_.Solve(constraints_, frozen_trajectories_,
                               landmark_nodes_);

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

@@ -257,6 +257,9 @@ class PoseGraph2D : public PoseGraph {
   // Set of all frozen trajectories not being optimized.
   std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
 
+  // Whether or not optimize landmark poses.
+  bool freeze_landmarks_ GUARDED_BY(mutex_) = false;
+
   // Set of all finished trajectories.
   std::set<int> finished_trajectories_ GUARDED_BY(mutex_);
 

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc

@@ -117,10 +117,15 @@ transform::Rigid3d GetInitialLandmarkPose(
 
 void AddLandmarkCostFunctions(
     const std::map<std::string, LandmarkNode>& landmark_nodes,
-    const MapById<NodeId, NodeData>& node_data,
+    bool freeze_landmarks, const MapById<NodeId, NodeData>& node_data,
     MapById<NodeId, CeresPose>* C_nodes,
     std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
   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 =
@@ -152,6 +157,12 @@ void AddLandmarkCostFunctions(
             CeresPose(starting_point, nullptr /* translation_parametrization */,
                       common::make_unique<ceres::QuaternionParameterization>(),
                       problem));
+        if (freeze_landmarks) {
+          problem->SetParameterBlockConstant(
+              C_landmarks->at(landmark_id).translation());
+          problem->SetParameterBlockConstant(
+              C_landmarks->at(landmark_id).rotation());
+        }
       }
       problem->AddResidualBlock(
           LandmarkCostFunction3D::CreateAutoDiffCostFunction(
@@ -267,6 +278,7 @@ 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;
@@ -326,8 +338,8 @@ void OptimizationProblem3D::Solve(
         C_nodes.at(constraint.node_id).translation());
   }
   // Add cost functions for landmarks.
-  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
+  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
-                           &problem);
+                           &C_nodes, &C_landmarks, &problem);
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
   if (fix_z_ == FixZ::kNo) {

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

@@ -593,8 +593,8 @@ void PoseGraph3D::RunOptimization() {
   }
 
   // No other thread is accessing the optimization_problem_, constraints_,
-  // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
+  // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
-  // is time consuming, so not taking the mutex before Solve to avoid blocking
+  // time consuming, so not taking the mutex before Solve to avoid blocking
   // foreground processing.
   optimization_problem_.Solve(constraints_, frozen_trajectories_,
                               landmark_nodes_);

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

@@ -261,6 +261,9 @@ class PoseGraph3D : public PoseGraph {
   // Set of all frozen trajectories not being optimized.
   std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
 
+  // Whether or not optimize landmark poses.
+  bool freeze_landmarks_ GUARDED_BY(mutex_) = false;
+
   // Set of all finished trajectories.
   std::set<int> finished_trajectories_ GUARDED_BY(mutex_);
 

cartographer/mapping/map_builder.cc

@@ -369,7 +369,11 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
             sensor::FromProto(
                 proto.fixed_frame_pose_data().fixed_frame_pose_data()));
       }
-      // TODO(pifon2a, ojura): deserialize landmarks
+      if (proto.has_landmark_data()) {
+        pose_graph_->AddLandmarkData(
+            trajectory_remapping.at(proto.landmark_data().trajectory_id()),
+            sensor::FromProto(proto.landmark_data().landmark_data()));
+      }
     }
   }
 

cartographer/mapping/proto/serialization.proto

@@ -48,6 +48,11 @@ message FixedFramePoseData {
   sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
 }
 
+message LandmarkData {
+  int32 trajectory_id = 1;
+  sensor.proto.LandmarkData landmark_data = 2;
+}
+
 message TrajectoryData {
   int32 trajectory_id = 1;
   double gravity_constant = 2;
@@ -62,6 +67,7 @@ message SerializedData {
   OdometryData odometry_data = 4;
   FixedFramePoseData fixed_frame_pose_data = 5;
   TrajectoryData trajectory_data = 6;
+  LandmarkData landmark_data = 7;
 }
 
 message LocalSlamResultData {