diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h
index 2db3b21..03083ef 100644
--- a/cartographer/mapping/pose_graph_interface.h
+++ b/cartographer/mapping/pose_graph_interface.h
@@ -82,6 +82,9 @@ class PoseGraphInterface {
   // Returns the current optimized trajectories.
   virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
 
+  // Returns the current optimized trajectory poses.
+  virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() = 0;
+
   // Checks if the given trajectory is finished.
   virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
 
diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h
index dea4d32..e0ee709 100644
--- a/cartographer/mapping/trajectory_node.h
+++ b/cartographer/mapping/trajectory_node.h
@@ -29,6 +29,14 @@
 namespace cartographer {
 namespace mapping {
 
+struct TrajectoryNodePose {
+  // Indicates whether the node has constant data set.
+  bool has_constant_data;
+
+  // The node pose in the global SLAM frame.
+  transform::Rigid3d global_pose;
+};
+
 struct TrajectoryNode {
   struct Data {
     common::Time time;
diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc
index 787c5ab..7579b46 100644
--- a/cartographer/mapping_2d/pose_graph.cc
+++ b/cartographer/mapping_2d/pose_graph.cc
@@ -575,6 +575,19 @@ PoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+PoseGraph::GetTrajectoryNodePoses() {
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
+  common::MutexLocker locker(&mutex_);
+  for (const auto& node_id_data : trajectory_nodes_) {
+    node_poses.Insert(
+        node_id_data.id,
+        mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
+                                    node_id_data.data.global_pose});
+  }
+  return node_poses;
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_.imu_data();
diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h
index 0514794..1b39d51 100644
--- a/cartographer/mapping_2d/pose_graph.h
+++ b/cartographer/mapping_2d/pose_graph.h
@@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+  GetTrajectoryNodePoses() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);
diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc
index 7e430d1..396e11c 100644
--- a/cartographer/mapping_3d/pose_graph.cc
+++ b/cartographer/mapping_3d/pose_graph.cc
@@ -605,6 +605,19 @@ PoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+PoseGraph::GetTrajectoryNodePoses() {
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
+  common::MutexLocker locker(&mutex_);
+  for (const auto& node_id_data : trajectory_nodes_) {
+    node_poses.Insert(
+        node_id_data.id,
+        mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
+                                    node_id_data.data.global_pose});
+  }
+  return node_poses;
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_.imu_data();
diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h
index a0863c4..ebc79f4 100644
--- a/cartographer/mapping_3d/pose_graph.h
+++ b/cartographer/mapping_3d/pose_graph.h
@@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+  GetTrajectoryNodePoses() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);
diff --git a/cartographer_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc
index bbb277c..1bcd5ff 100644
--- a/cartographer_grpc/mapping/pose_graph_stub.cc
+++ b/cartographer_grpc/mapping/pose_graph_stub.cc
@@ -52,6 +52,12 @@ PoseGraphStub::GetTrajectoryNodes() {
   LOG(FATAL) << "Not implemented";
 }
 
+cartographer::mapping::MapById<cartographer::mapping::NodeId,
+                               cartographer::mapping::TrajectoryNodePose>
+PoseGraphStub::GetTrajectoryNodePoses() {
+  LOG(FATAL) << "Not implemented";
+}
+
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
   LOG(FATAL) << "Not implemented";
 }
diff --git a/cartographer_grpc/mapping/pose_graph_stub.h b/cartographer_grpc/mapping/pose_graph_stub.h
index f063c48..e85e1f0 100644
--- a/cartographer_grpc/mapping/pose_graph_stub.h
+++ b/cartographer_grpc/mapping/pose_graph_stub.h
@@ -42,6 +42,9 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
   cartographer::mapping::MapById<cartographer::mapping::NodeId,
                                  cartographer::mapping::TrajectoryNode>
   GetTrajectoryNodes() override;
+  cartographer::mapping::MapById<cartographer::mapping::NodeId,
+                                 cartographer::mapping::TrajectoryNodePose>
+  GetTrajectoryNodePoses() override;
   bool IsTrajectoryFinished(int trajectory_id) override;
   std::vector<Constraint> constraints() override;