From 4cd812d8060ff57cb12beabbd59015935f978eb7 Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Tue, 16 May 2017 15:07:31 +0200
Subject: [PATCH] Track googlecartographer/cartographer#290. (#340)

PAIR=wohe
---
 cartographer_ros/cartographer_ros/map_builder_bridge.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index 3e8cf0c..9c4b4bb 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -112,10 +112,10 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   for (int trajectory_id = 0;
        trajectory_id < map_builder_.num_trajectory_builders();
        ++trajectory_id) {
+    const std::vector<cartographer::transform::Rigid3d> submap_transforms =
+        map_builder_.sparse_pose_graph()->GetSubmapTransforms(trajectory_id);
     const cartographer::mapping::Submaps* submaps =
         map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps();
-    const std::vector<cartographer::transform::Rigid3d> submap_transforms =
-        map_builder_.sparse_pose_graph()->GetSubmapTransforms(submaps);
     CHECK_LE(submap_transforms.size(), submaps->size());
 
     cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list;
@@ -172,7 +172,7 @@ MapBuilderBridge::GetTrajectoryStates() {
     trajectory_states[trajectory_id] = {
         pose_estimate,
         map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
-            trajectory_builder->submaps()),
+            trajectory_id),
         sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time,
                                                    options_.published_frame)};
   }