diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index 786c80f..c811692 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -150,7 +150,8 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   cartographer_ros_msgs::SubmapList submap_list;
   submap_list.header.stamp = ::ros::Time::now();
   submap_list.header.frame_id = node_options_.map_frame;
-  for (const auto& submap_id_data : map_builder_.sparse_pose_graph()->GetAllSubmapData()) {
+  for (const auto &submap_id_data :
+       map_builder_.sparse_pose_graph()->GetAllSubmapData()) {
     cartographer_ros_msgs::SubmapEntry submap_entry;
     submap_entry.trajectory_id = submap_id_data.id.trajectory_id;
     submap_entry.submap_index = submap_id_data.id.submap_index;
@@ -192,22 +193,18 @@ MapBuilderBridge::GetTrajectoryStates() {
 
 visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
   visualization_msgs::MarkerArray trajectory_node_list;
-  const auto all_trajectory_nodes =
-      map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
-  for (int trajectory_id = 0;
-       trajectory_id < static_cast<int>(all_trajectory_nodes.size());
-       ++trajectory_id) {
-    const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
+  const auto nodes = map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
+  for (const int trajectory_id : nodes.trajectory_ids()) {
     visualization_msgs::Marker marker =
         CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
 
-    for (const auto& node : single_trajectory_nodes) {
-      if (node.constant_data == nullptr) {
+    for (const auto& node_id_data : nodes.trajectory(trajectory_id)) {
+      if (node_id_data.data.constant_data == nullptr) {
         PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
         continue;
       }
       const ::geometry_msgs::Point node_point =
-          ToGeometryMsgPoint(node.global_pose.translation());
+          ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
       marker.points.push_back(node_point);
       // Work around the 16384 point limit in RViz by splitting the
       // trajectory into multiple markers.
@@ -252,10 +249,9 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
   residual_inter_marker.ns = "Inter residuals";
   residual_inter_marker.pose.position.z = 0.1;
 
-  const auto all_trajectory_nodes =
+  const auto trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
-  const auto submap_data =
-      map_builder_.sparse_pose_graph()->GetAllSubmapData();
+  const auto submap_data = map_builder_.sparse_pose_graph()->GetAllSubmapData();
   const auto constraints = map_builder_.sparse_pose_graph()->constraints();
 
   for (const auto& constraint : constraints) {
@@ -294,10 +290,11 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
       continue;
     }
     const auto& submap_pose = submap_it->data.pose;
-    const auto& trajectory_node_pose =
-        all_trajectory_nodes[constraint.node_id.trajectory_id]
-                            [constraint.node_id.node_index]
-                                .global_pose;
+    const auto node_it = trajectory_nodes.find(constraint.node_id);
+    if (node_it == trajectory_nodes.end()) {
+      continue;
+    }
+    const auto& trajectory_node_pose = node_it->data.global_pose;
     const cartographer::transform::Rigid3d constraint_pose =
         submap_pose * constraint.pose.zbar_ij;