diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc
index 15272ea..1956ce9 100644
--- a/cartographer/mapping_2d/pose_graph.cc
+++ b/cartographer/mapping_2d/pose_graph.cc
@@ -80,8 +80,8 @@ std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
   CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
   const mapping::SubmapId last_submap_id = std::prev(end_it)->id;
   if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
-    // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
-    // and 'insertions_submaps.back()' is new.
+    // In this case, 'last_submap_id' is the ID of
+    // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
     const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
     optimization_problem_.AddSubmap(
         trajectory_id,
@@ -584,6 +584,9 @@ void PoseGraph::RunOptimization() {
           old_global_to_new_global * mutable_trajectory_node.global_pose;
     }
   }
+  for (const auto& landmark : optimization_problem_.landmark_data()) {
+    landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
+  }
   global_submap_poses_ = submap_data;
 }
 
diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.cc b/cartographer/mapping_2d/pose_graph/optimization_problem.cc
index 47bd561..ad074fb 100644
--- a/cartographer/mapping_2d/pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_2d/pose_graph/optimization_problem.cc
@@ -278,7 +278,10 @@ void OptimizationProblem::Solve(
   for (const auto& C_node_id_data : C_nodes) {
     node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data);
   }
-}  // namespace pose_graph
+  for (const auto& C_landmark : C_landmarks) {
+    landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
+  }
+}
 
 const mapping::MapById<mapping::NodeId, NodeData>&
 OptimizationProblem::node_data() const {
@@ -290,7 +293,7 @@ OptimizationProblem::submap_data() const {
   return submap_data_;
 }
 
-const std::map<std::string, transform::Rigid2d>&
+const std::map<std::string, transform::Rigid3d>&
 OptimizationProblem::landmark_data() const {
   return landmark_data_;
 }
diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.h b/cartographer/mapping_2d/pose_graph/optimization_problem.h
index 98ebe6a..68ab147 100644
--- a/cartographer/mapping_2d/pose_graph/optimization_problem.h
+++ b/cartographer/mapping_2d/pose_graph/optimization_problem.h
@@ -90,7 +90,7 @@ class OptimizationProblem {
 
   const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
   const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
-  const std::map<std::string, transform::Rigid2d>& landmark_data() const;
+  const std::map<std::string, transform::Rigid3d>& landmark_data() const;
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
   const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
 
@@ -105,7 +105,7 @@ class OptimizationProblem {
   mapping::pose_graph::proto::OptimizationProblemOptions options_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
-  std::map<std::string, transform::Rigid2d> landmark_data_;
+  std::map<std::string, transform::Rigid3d> landmark_data_;
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
 };
diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc
index f3b0150..a6f368e 100644
--- a/cartographer/mapping_3d/pose_graph.cc
+++ b/cartographer/mapping_3d/pose_graph.cc
@@ -608,6 +608,9 @@ void PoseGraph::RunOptimization() {
           old_global_to_new_global * mutable_trajectory_node.global_pose;
     }
   }
+  for (const auto& landmark : optimization_problem_.landmark_data()) {
+    landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
+  }
   global_submap_poses_ = submap_data;
 
   // Log the histograms for the pose residuals.
diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
index 211a333..36aa261 100644
--- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
@@ -535,7 +535,10 @@ void OptimizationProblem::Solve(
     trajectory_data_.at(C_fixed_frame.first).fixed_frame =
         C_fixed_frame.second.ToRigid();
   }
-}  // namespace pose_graph
+  for (const auto& C_landmark : C_landmarks) {
+    landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
+  }
+}
 
 const mapping::MapById<mapping::NodeId, NodeData>&
 OptimizationProblem::node_data() const {