Fix race condition in extrapolation of submap poses. (#268)

PAIR=wohe

cartographer/mapping/map_builder.cc

@@ -128,17 +128,17 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
 
   const Submaps* const submaps =
       trajectory_builders_.at(trajectory_id)->submaps();
-  if (submap_index < 0 || submap_index >= submaps->size()) {
+  const std::vector<transform::Rigid3d> submap_transforms =
+      sparse_pose_graph_->GetSubmapTransforms(submaps);
+  if (submap_index < 0 ||
+      static_cast<size_t>(submap_index) >= submap_transforms.size()) {
     return "Requested submap " + std::to_string(submap_index) +
            " from trajectory " + std::to_string(trajectory_id) +
-           " but there are only " + std::to_string(submaps->size()) +
+           " but there are only " + std::to_string(submap_transforms.size()) +
            " submaps in this trajectory.";
   }
 
   response->set_submap_version(submaps->Get(submap_index)->num_range_data);
-  const std::vector<transform::Rigid3d> submap_transforms =
-      sparse_pose_graph_->GetSubmapTransforms(*submaps);
-  CHECK_EQ(submap_transforms.size(), submaps->size());
   submaps->SubmapToProto(submap_index, sparse_pose_graph_->GetTrajectoryNodes(),
                          submap_transforms[submap_index], response);
   return "";

cartographer/mapping/sparse_pose_graph.cc

@@ -150,8 +150,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
           transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
     }
 
-    const Submaps* const submaps = group[0].constant_data->trajectory;
+    const Submaps* const trajectory = group[0].constant_data->trajectory;
-    for (const auto& transform : GetSubmapTransforms(*submaps)) {
+    for (const auto& transform : GetSubmapTransforms(trajectory)) {
       *trajectory_proto->add_submap()->mutable_pose() =
           transform::ToProto(transform);
     }

cartographer/mapping/sparse_pose_graph.h

@@ -109,15 +109,15 @@ class SparsePoseGraph {
   virtual std::vector<std::vector<const Submaps*>>
   GetConnectedTrajectories() = 0;
 
-  // Returns the current optimized transforms for the given 'submaps'.
+  // Returns the current optimized transforms for the given 'trajectory'.
   virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
-      const Submaps& submaps) = 0;
+      const Submaps* trajectory) = 0;
 
   // Returns the transform converting data in the local map frame (i.e. the
   // continuous, non-loop-closed frame) into the global map frame (i.e. the
   // discontinuous, loop-closed frame).
   virtual transform::Rigid3d GetLocalToGlobalTransform(
-      const Submaps& submaps) = 0;
+      const Submaps* submaps) = 0;
 
   // Returns the current optimized trajectory.
   virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;

cartographer/mapping/submaps.h

@@ -75,7 +75,7 @@ struct Submap {
   }
 
   // Origin of this submap.
-  Eigen::Vector3f origin;
+  const Eigen::Vector3f origin;
 
   // Number of RangeData inserted.
   int num_range_data = 0;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -84,11 +84,11 @@ void SparsePoseGraph::AddScan(
     common::Time time, const transform::Rigid3d& tracking_to_pose,
     const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
     const kalman_filter::Pose2DCovariance& covariance,
-    const mapping::Submaps* submaps,
+    const mapping::Submaps* const trajectory,
     const mapping::Submap* const matching_submap,
     const std::vector<const mapping::Submap*>& insertion_submaps) {
-  const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) *
+  const transform::Rigid3d optimized_pose(
-                                          transform::Embed3D(pose));
+      GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose));
 
   common::MutexLocker locker(&mutex_);
   const int j = trajectory_nodes_.size();
@@ -96,19 +96,19 @@ void SparsePoseGraph::AddScan(
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, range_data_in_pose,
-      Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps,
+      Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory,
       tracking_to_pose});
   trajectory_nodes_.push_back(mapping::TrajectoryNode{
       &constant_node_data_.back(), optimized_pose,
   });
-  trajectory_connectivity_.Add(submaps);
+  trajectory_connectivity_.Add(trajectory);
 
   if (submap_indices_.count(insertion_submaps.back()) == 0) {
     submap_indices_.emplace(insertion_submaps.back(),
                             static_cast<int>(submap_indices_.size()));
     submap_states_.emplace_back();
     submap_states_.back().submap = insertion_submaps.back();
-    submap_states_.back().trajectory = submaps;
+    submap_states_.back().trajectory = trajectory;
     CHECK_EQ(submap_states_.size(), submap_indices_.size());
   }
   const mapping::Submap* const finished_submap =
@@ -117,8 +117,8 @@ void SparsePoseGraph::AddScan(
           : nullptr;
 
   // Make sure we have a sampler for this trajectory.
-  if (!global_localization_samplers_[submaps]) {
+  if (!global_localization_samplers_[trajectory]) {
-    global_localization_samplers_[submaps] =
+    global_localization_samplers_[trajectory] =
         common::make_unique<common::FixedRatioSampler>(
             options_.global_sampling_ratio());
   }
@@ -347,13 +347,14 @@ void SparsePoseGraph::RunOptimization() {
       const mapping::Submaps* trajectory =
           trajectory_nodes_[i].constant_data->trajectory;
       if (extrapolation_transforms.count(trajectory) == 0) {
-        extrapolation_transforms[trajectory] = transform::Rigid3d(
+        const auto new_submap_transforms =
-            ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
+            ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
-                .back() *
+        const auto old_submap_transforms = ExtrapolateSubmapTransforms(
-            ExtrapolateSubmapTransforms(optimized_submap_transforms_,
+            optimized_submap_transforms_, trajectory);
-                                        *trajectory)
+        CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-                .back()
+        extrapolation_transforms[trajectory] =
-                .inverse());
+            transform::Rigid3d(new_submap_transforms.back() *
+                               old_submap_transforms.back().inverse());
       }
       trajectory_nodes_[i].pose =
           extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
@@ -384,9 +385,12 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
 }
 
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
-    const mapping::Submaps& submaps) {
+    const mapping::Submaps* const submaps) {
-  return GetSubmapTransforms(submaps).back() *
+  const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
-         submaps.Get(submaps.size() - 1)->local_pose().inverse();
+  return extrapolated_submap_transforms.back() *
+         submaps->Get(extrapolated_submap_transforms.size() - 1)
+             ->local_pose()
+             .inverse();
 }
 
 std::vector<std::vector<const mapping::Submaps*>>
@@ -396,39 +400,51 @@ SparsePoseGraph::GetConnectedTrajectories() {
 }
 
 std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
-    const mapping::Submaps& submaps) {
+    const mapping::Submaps* const trajectory) {
   common::MutexLocker locker(&mutex_);
-  return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps);
+  return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
 }
 
 std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     const std::vector<transform::Rigid2d>& submap_transforms,
-    const mapping::Submaps& submaps) const {
+    const mapping::Submaps* const trajectory) const {
   std::vector<transform::Rigid3d> result;
-  int i = 0;
+  size_t flat_index = 0;
+  size_t flat_index_of_result_back = -1;
+
   // Submaps for which we have optimized poses.
-  for (; i < submaps.size(); ++i) {
+  for (; flat_index != submap_states_.size(); ++flat_index) {
-    const mapping::Submap* submap = submaps.Get(i);
+    const auto& state = submap_states_[flat_index];
-    const auto submap_and_index = submap_indices_.find(submap);
+    if (state.trajectory != trajectory) {
-    // Determine if we have a loop-closed transform for this submap.
+      continue;
-    if (submap_and_index == submap_indices_.end() ||
+    }
-        static_cast<size_t>(submap_and_index->second) >=
+    if (flat_index >= submap_transforms.size()) {
-            submap_transforms.size()) {
       break;
     }
-    result.push_back(
+    result.push_back(transform::Embed3D(submap_transforms[flat_index]));
-        transform::Embed3D(submap_transforms[submap_and_index->second]));
+    flat_index_of_result_back = flat_index;
   }
 
   // Extrapolate to the remaining submaps.
-  for (; i < submaps.size(); ++i) {
+  for (; flat_index != submap_states_.size(); ++flat_index) {
-    if (i == 0) {
+    const auto& state = submap_states_[flat_index];
-      result.push_back(transform::Rigid3d::Identity());
+    if (state.trajectory != trajectory) {
       continue;
     }
-    result.push_back(result.back() *
+    if (result.empty()) {
-                     submaps.Get(result.size() - 1)->local_pose().inverse() *
+      result.push_back(transform::Rigid3d::Identity());
-                     submaps.Get(result.size())->local_pose());
+    } else {
+      // Accessing local_pose() in Submaps is okay, since the member is const.
+      result.push_back(result.back() *
+                       submap_states_[flat_index_of_result_back]
+                           .submap->local_pose()
+                           .inverse() *
+                       state.submap->local_pose());
+    }
+    flat_index_of_result_back = flat_index;
+  }
+  if (result.empty()) {
+    result.push_back(transform::Rigid3d::Identity());
   }
   return result;
 }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -72,7 +72,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                const sensor::RangeData& range_data_in_pose,
                const transform::Rigid2d& pose,
                const kalman_filter::Pose2DCovariance& pose_covariance,
-               const mapping::Submaps* submaps,
+               const mapping::Submaps* trajectory,
                const mapping::Submap* matching_submap,
                const std::vector<const mapping::Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
@@ -86,8 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
       override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(
-      const mapping::Submaps& submaps) EXCLUDES(mutex_) override;
+      const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
-  transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps)
+  transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
@@ -139,7 +139,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Adds extrapolated transforms, so that there are transforms for all submaps.
   std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
       const std::vector<transform::Rigid2d>& submap_transforms,
-      const mapping::Submaps& submaps) const REQUIRES(mutex_);
+      const mapping::Submaps* trajectory) const REQUIRES(mutex_);
 
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -83,11 +83,11 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 void SparsePoseGraph::AddScan(
     common::Time time, const sensor::RangeData& range_data_in_tracking,
     const transform::Rigid3d& pose,
-    const kalman_filter::PoseCovariance& covariance, const Submaps* submaps,
+    const kalman_filter::PoseCovariance& covariance,
-    const Submap* const matching_submap,
+    const Submaps* const trajectory, const Submap* const matching_submap,
     const std::vector<const Submap*>& insertion_submaps) {
-  const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) *
+  const transform::Rigid3d optimized_pose(
-                                          pose);
+      GetLocalToGlobalTransform(trajectory) * pose);
 
   common::MutexLocker locker(&mutex_);
   const int j = trajectory_nodes_.size();
@@ -95,26 +95,26 @@ void SparsePoseGraph::AddScan(
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
-      sensor::Compress(range_data_in_tracking), submaps,
+      sensor::Compress(range_data_in_tracking), trajectory,
       transform::Rigid3d::Identity()});
   trajectory_nodes_.push_back(
       mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
-  trajectory_connectivity_.Add(submaps);
+  trajectory_connectivity_.Add(trajectory);
 
   if (submap_indices_.count(insertion_submaps.back()) == 0) {
     submap_indices_.emplace(insertion_submaps.back(),
                             static_cast<int>(submap_indices_.size()));
     submap_states_.emplace_back();
     submap_states_.back().submap = insertion_submaps.back();
-    submap_states_.back().trajectory = submaps;
+    submap_states_.back().trajectory = trajectory;
     CHECK_EQ(submap_states_.size(), submap_indices_.size());
   }
   const Submap* const finished_submap =
       insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
 
   // Make sure we have a sampler for this trajectory.
-  if (!global_localization_samplers_[submaps]) {
+  if (!global_localization_samplers_[trajectory]) {
-    global_localization_samplers_[submaps] =
+    global_localization_samplers_[trajectory] =
         common::make_unique<common::FixedRatioSampler>(
             options_.global_sampling_ratio());
   }
@@ -341,13 +341,14 @@ void SparsePoseGraph::RunOptimization() {
       const mapping::Submaps* trajectory =
           trajectory_nodes_[i].constant_data->trajectory;
       if (extrapolation_transforms.count(trajectory) == 0) {
-        extrapolation_transforms[trajectory] = transform::Rigid3d(
+        const auto new_submap_transforms =
-            ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
+            ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
-                .back() *
+        const auto old_submap_transforms = ExtrapolateSubmapTransforms(
-            ExtrapolateSubmapTransforms(optimized_submap_transforms_,
+            optimized_submap_transforms_, trajectory);
-                                        *trajectory)
+        CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-                .back()
+        extrapolation_transforms[trajectory] =
-                .inverse());
+            transform::Rigid3d(new_submap_transforms.back() *
+                               old_submap_transforms.back().inverse());
       }
       trajectory_nodes_[i].pose =
           extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
@@ -388,9 +389,12 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
 }
 
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
-    const mapping::Submaps& submaps) {
+    const mapping::Submaps* const submaps) {
-  return GetSubmapTransforms(submaps).back() *
+  const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
-         submaps.Get(submaps.size() - 1)->local_pose().inverse();
+  return extrapolated_submap_transforms.back() *
+         submaps->Get(extrapolated_submap_transforms.size() - 1)
+             ->local_pose()
+             .inverse();
 }
 
 std::vector<std::vector<const mapping::Submaps*>>
@@ -400,38 +404,51 @@ SparsePoseGraph::GetConnectedTrajectories() {
 }
 
 std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
-    const mapping::Submaps& submaps) {
+    const mapping::Submaps* const trajectory) {
   common::MutexLocker locker(&mutex_);
-  return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps);
+  return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
 }
 
 std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     const std::vector<transform::Rigid3d>& submap_transforms,
-    const mapping::Submaps& submaps) const {
+    const mapping::Submaps* const trajectory) const {
   std::vector<transform::Rigid3d> result;
-  int i = 0;
+  size_t flat_index = 0;
+  size_t flat_index_of_result_back = -1;
+
   // Submaps for which we have optimized poses.
-  for (; i < submaps.size(); ++i) {
+  for (; flat_index != submap_states_.size(); ++flat_index) {
-    const mapping::Submap* submap = submaps.Get(i);
+    const auto& state = submap_states_[flat_index];
-    const auto submap_and_index = submap_indices_.find(submap);
+    if (state.trajectory != trajectory) {
-    // Determine if we have a loop-closed transform for this submap.
+      continue;
-    if (submap_and_index == submap_indices_.end() ||
+    }
-        static_cast<size_t>(submap_and_index->second) >=
+    if (flat_index >= submap_transforms.size()) {
-            submap_transforms.size()) {
       break;
     }
-    result.push_back(submap_transforms[submap_and_index->second]);
+    result.push_back(submap_transforms[flat_index]);
+    flat_index_of_result_back = flat_index;
   }
 
   // Extrapolate to the remaining submaps.
-  for (; i < submaps.size(); ++i) {
+  for (; flat_index != submap_states_.size(); ++flat_index) {
-    if (i == 0) {
+    const auto& state = submap_states_[flat_index];
-      result.push_back(transform::Rigid3d::Identity());
+    if (state.trajectory != trajectory) {
       continue;
     }
-    result.push_back(result.back() *
+    if (result.empty()) {
-                     submaps.Get(result.size() - 1)->local_pose().inverse() *
+      result.push_back(transform::Rigid3d::Identity());
-                     submaps.Get(result.size())->local_pose());
+    } else {
+      // Accessing local_pose() in Submaps is okay, since the member is const.
+      result.push_back(result.back() *
+                       submap_states_[flat_index_of_result_back]
+                           .submap->local_pose()
+                           .inverse() *
+                       state.submap->local_pose());
+    }
+    flat_index_of_result_back = flat_index;
+  }
+  if (result.empty()) {
+    result.push_back(transform::Rigid3d::Identity());
   }
   return result;
 }

cartographer/mapping_3d/sparse_pose_graph.h

@@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                const sensor::RangeData& range_data_in_tracking,
                const transform::Rigid3d& pose,
                const kalman_filter::PoseCovariance& pose_covariance,
-               const Submaps* submaps, const Submap* matching_submap,
+               const Submaps* trajectory, const Submap* matching_submap,
                const std::vector<const Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -88,8 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
       override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(
-      const mapping::Submaps& submaps) EXCLUDES(mutex_) override;
+      const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
-  transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps)
+  transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
@@ -159,7 +159,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Adds extrapolated transforms, so that there are transforms for all submaps.
   std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
       const std::vector<transform::Rigid3d>& submap_transforms,
-      const mapping::Submaps& submaps) const REQUIRES(mutex_);
+      const mapping::Submaps* trajectory) const REQUIRES(mutex_);
 
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;

cartographer/mapping_3d/submaps.h

@@ -109,6 +109,8 @@ class Submaps : public mapping::Submaps {
 
   const proto::SubmapsOptions options_;
 
+  // 'submaps_' contains pointers, so that resizing the vector does not
+  // invalidate handed out Submap* pointers.
   std::vector<std::unique_ptr<Submap>> submaps_;
   RangeDataInserter range_data_inserter_;
 };