Fix crashing bug in localization. (#575)

Now that we deserialize trajectory nodes, we need to make sure
that the global matcher sampler is added for the map trajectory.

cartographer/io/frame_id_filtering_points_processor.cc

@@ -26,8 +26,7 @@ namespace io {
 
 std::unique_ptr<FrameIdFilteringPointsProcessor>
 FrameIdFilteringPointsProcessor::FromDictionary(
-    common::LuaParameterDictionary* dictionary,
+    common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
-    PointsProcessor* next) {
   std::vector<string> keep_frames, drop_frames;
   if (dictionary->HasKey("keep_frames")) {
     keep_frames =
@@ -44,8 +43,7 @@ FrameIdFilteringPointsProcessor::FromDictionary(
 
 FrameIdFilteringPointsProcessor::FrameIdFilteringPointsProcessor(
     const std::unordered_set<string>& keep_frame_ids,
-    const std::unordered_set<string>& drop_frame_ids,
+    const std::unordered_set<string>& drop_frame_ids, PointsProcessor* next)
-    PointsProcessor* next)
     : keep_frame_ids_(keep_frame_ids),
       drop_frame_ids_(drop_frame_ids),
       next_(next) {

cartographer/io/frame_id_filtering_points_processor.h

@@ -33,11 +33,9 @@ class FrameIdFilteringPointsProcessor : public PointsProcessor {
   constexpr static const char* kConfigurationFileActionName = "frame_id_filter";
   FrameIdFilteringPointsProcessor(
       const std::unordered_set<string>& keep_frame_ids,
-      const std::unordered_set<string>& drop_frame_ids,
+      const std::unordered_set<string>& drop_frame_ids, PointsProcessor* next);
-      PointsProcessor* next);
   static std::unique_ptr<FrameIdFilteringPointsProcessor> FromDictionary(
-      common::LuaParameterDictionary* dictionary,
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
-      PointsProcessor* next);
   ~FrameIdFilteringPointsProcessor() override {}
 
   FrameIdFilteringPointsProcessor(const FrameIdFilteringPointsProcessor&) =

cartographer/io/points_processor_pipeline_builder.cc

@@ -19,8 +19,8 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/io/coloring_points_processor.h"
 #include "cartographer/io/counting_points_processor.h"
-#include "cartographer/io/frame_id_filtering_points_processor.h"
 #include "cartographer/io/fixed_ratio_sampling_points_processor.h"
+#include "cartographer/io/frame_id_filtering_points_processor.h"
 #include "cartographer/io/hybrid_grid_points_processor.h"
 #include "cartographer/io/intensity_to_color_points_processor.h"
 #include "cartographer/io/min_max_range_filtering_points_processor.h"

cartographer/mapping/id.h

@@ -33,7 +33,7 @@ struct NodeId {
 
   bool operator==(const NodeId& other) const {
     return std::forward_as_tuple(trajectory_id, node_index) ==
-        std::forward_as_tuple(other.trajectory_id, other.node_index);
+           std::forward_as_tuple(other.trajectory_id, other.node_index);
   }
 
   bool operator!=(const NodeId& other) const { return !operator==(other); }

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -102,10 +102,10 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
 
   common::MutexLocker locker(&mutex_);
+  AddTrajectoryIfNeeded(trajectory_id);
   trajectory_nodes_.Append(
       trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
   ++num_trajectory_nodes_;
-  trajectory_connectivity_state_.Add(trajectory_id);
 
   // Test if the 'insertion_submap.back()' is one we never saw before.
   if (trajectory_id >= submap_data_.num_trajectories() ||
@@ -121,13 +121,6 @@ void SparsePoseGraph::AddScan(
     submap_data_.at(submap_id).submap = insertion_submaps.back();
   }
 
-  // Make sure we have a sampler for this trajectory.
-  if (!global_localization_samplers_[trajectory_id]) {
-    global_localization_samplers_[trajectory_id] =
-        common::make_unique<common::FixedRatioSampler>(
-            options_.global_sampling_ratio());
-  }
-
   // We have to check this here, because it might have changed by the time we
   // execute the lambda.
   const bool newly_finished_submap = insertion_submaps.front()->finished();
@@ -145,6 +138,16 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
   }
 }
 
+void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
+  trajectory_connectivity_state_.Add(trajectory_id);
+  // Make sure we have a sampler for this trajectory.
+  if (!global_localization_samplers_[trajectory_id]) {
+    global_localization_samplers_[trajectory_id] =
+        common::make_unique<common::FixedRatioSampler>(
+            options_.global_sampling_ratio());
+  }
+}
+
 void SparsePoseGraph::AddImuData(const int trajectory_id,
                                  const sensor::ImuData& imu_data) {
   common::MutexLocker locker(&mutex_);
@@ -232,15 +235,15 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
       static_cast<size_t>(matching_id.trajectory_id) <
-          optimization_problem_.node_data().size() &&
+                  optimization_problem_.node_data().size() &&
-          !optimization_problem_.node_data()[matching_id.trajectory_id]
+              !optimization_problem_.node_data()[matching_id.trajectory_id]
-              .empty()
+                   .empty()
-      ? static_cast<int>(optimization_problem_.node_data()
+          ? static_cast<int>(optimization_problem_.node_data()
-          .at(matching_id.trajectory_id)
+                                 .at(matching_id.trajectory_id)
-          .rbegin()
+                                 .rbegin()
-          ->first +
+                                 ->first +
-          1)
+                             1)
-      : 0};
+          : 0};
   const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
   const transform::Rigid2d pose = transform::Project2D(
       constant_data->initial_pose *
@@ -419,7 +422,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
   const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose);
 
   common::MutexLocker locker(&mutex_);
-  trajectory_connectivity_state_.Add(trajectory_id);
+  AddTrajectoryIfNeeded(trajectory_id);
   const mapping::SubmapId submap_id =
       submap_data_.Append(trajectory_id, SubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
@@ -447,7 +450,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
           mapping::FromProto(node.node_data()));
 
   common::MutexLocker locker(&mutex_);
-  trajectory_connectivity_state_.Add(trajectory_id);
+  AddTrajectoryIfNeeded(trajectory_id);
   const mapping::NodeId node_id = trajectory_nodes_.Append(
       trajectory_id, mapping::TrajectoryNode{constant_data, pose});
 

cartographer/mapping_2d/sparse_pose_graph.h

@@ -39,6 +39,7 @@
 #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_2d/submaps.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
+#include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -121,6 +122,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Handles a new work item.
   void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
 
+  // Adds connectivity and sampler for a trajectory if it does not exist.
+  void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);
+
   // Grows the optimization problem to have an entry for every element of
   // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
   std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -100,10 +100,10 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
 
   common::MutexLocker locker(&mutex_);
+  AddTrajectoryIfNeeded(trajectory_id);
   trajectory_nodes_.Append(
       trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
   ++num_trajectory_nodes_;
-  trajectory_connectivity_state_.Add(trajectory_id);
 
   // Test if the 'insertion_submap.back()' is one we never saw before.
   if (trajectory_id >= submap_data_.num_trajectories() ||
@@ -119,13 +119,6 @@ void SparsePoseGraph::AddScan(
     submap_data_.at(submap_id).submap = insertion_submaps.back();
   }
 
-  // Make sure we have a sampler for this trajectory.
-  if (!global_localization_samplers_[trajectory_id]) {
-    global_localization_samplers_[trajectory_id] =
-        common::make_unique<common::FixedRatioSampler>(
-            options_.global_sampling_ratio());
-  }
-
   // We have to check this here, because it might have changed by the time we
   // execute the lambda.
   const bool newly_finished_submap = insertion_submaps.front()->finished();
@@ -143,6 +136,16 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
   }
 }
 
+void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
+  trajectory_connectivity_state_.Add(trajectory_id);
+  // Make sure we have a sampler for this trajectory.
+  if (!global_localization_samplers_[trajectory_id]) {
+    global_localization_samplers_[trajectory_id] =
+        common::make_unique<common::FixedRatioSampler>(
+            options_.global_sampling_ratio());
+  }
+}
+
 void SparsePoseGraph::AddImuData(const int trajectory_id,
                                  const sensor::ImuData& imu_data) {
   common::MutexLocker locker(&mutex_);
@@ -258,15 +261,15 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
       static_cast<size_t>(matching_id.trajectory_id) <
-          optimization_problem_.node_data().size() &&
+                  optimization_problem_.node_data().size() &&
-          !optimization_problem_.node_data()[matching_id.trajectory_id]
+              !optimization_problem_.node_data()[matching_id.trajectory_id]
-              .empty()
+                   .empty()
-      ? static_cast<int>(optimization_problem_.node_data()
+          ? static_cast<int>(optimization_problem_.node_data()
-          .at(matching_id.trajectory_id)
+                                 .at(matching_id.trajectory_id)
-          .rbegin()
+                                 .rbegin()
-          ->first +
+                                 ->first +
-          1)
+                             1)
-      : 0};
+          : 0};
   const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
   const transform::Rigid3d& pose = constant_data->initial_pose;
   const transform::Rigid3d optimized_pose =
@@ -438,7 +441,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
       std::make_shared<const Submap>(submap.submap_3d());
 
   common::MutexLocker locker(&mutex_);
-  trajectory_connectivity_state_.Add(trajectory_id);
+  AddTrajectoryIfNeeded(trajectory_id);
   const mapping::SubmapId submap_id =
       submap_data_.Append(trajectory_id, SubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
@@ -466,7 +469,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
           mapping::FromProto(node.node_data()));
 
   common::MutexLocker locker(&mutex_);
-  trajectory_connectivity_state_.Add(trajectory_id);
+  AddTrajectoryIfNeeded(trajectory_id);
   const mapping::NodeId node_id = trajectory_nodes_.Append(
       trajectory_id, mapping::TrajectoryNode{constant_data, pose});
 

cartographer/mapping_3d/sparse_pose_graph.h

@@ -122,6 +122,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Handles a new work item.
   void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
 
+  // Adds connectivity and sampler for a trajectory if it does not exist.
+  void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);
+
   // Grows the optimization problem to have an entry for every element of
   // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
   std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(