Introduce MatchingResult for LocalTrajectoryBuilder::AddRangeData. (#619)

In preparation for #574. Depends on ~~#618~~ (merged) and ~~#617~~ (merged).

cartographer/mapping/global_trajectory_builder.h

@@ -47,14 +47,21 @@ class GlobalTrajectoryBuilder
   void AddRangefinderData(const common::Time time,
                           const Eigen::Vector3f& origin,
                           const sensor::TimedPointCloud& ranges) override {
-    std::unique_ptr<typename LocalTrajectoryBuilder::InsertionResult>
+    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
-        insertion_result = local_trajectory_builder_.AddRangeData(
+        matching_result = local_trajectory_builder_.AddRangeData(
             time, sensor::TimedRangeData{origin, ranges, {}});
-    if (insertion_result == nullptr) {
+    if (matching_result == nullptr) {
+      // The range data has not been fully accumulated yet.
       return;
     }
-    sparse_pose_graph_->AddScan(insertion_result->constant_data, trajectory_id_,
+    std::unique_ptr<mapping::NodeId> node_id;
-                                insertion_result->insertion_submaps);
+    if (matching_result->insertion_result != nullptr) {
+      node_id = ::cartographer::common::make_unique<mapping::NodeId>(
+          sparse_pose_graph_->AddScan(
+              matching_result->insertion_result->constant_data, trajectory_id_,
+              matching_result->insertion_result->insertion_submaps));
+      CHECK_EQ(node_id->trajectory_id, trajectory_id_);
+    }
   }
 
   void AddSensorData(const sensor::ImuData& imu_data) override {

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -73,7 +73,7 @@ void LocalTrajectoryBuilder::ScanMatch(
       matching_submap->probability_grid(), pose_observation, &summary);
 }
 
-std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 LocalTrajectoryBuilder::AddRangeData(const common::Time time,
                                      const sensor::TimedRangeData& range_data) {
   // Initialize extrapolator now if we do not ever use an IMU.
@@ -126,7 +126,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
   return nullptr;
 }
 
-std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
     const common::Time time, const sensor::RangeData& range_data) {
   // Transforms 'range_data' from the tracking frame into a frame where gravity
@@ -158,8 +158,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       TransformRangeData(gravity_aligned_range_data,
                          transform::Embed3D(pose_estimate_2d.cast<float>()));
   last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
-  return InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
+  std::unique_ptr<InsertionResult> insertion_result =
-                          pose_estimate, gravity_alignment.rotation());
+      InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
+                       pose_estimate, gravity_alignment.rotation());
+  return common::make_unique<MatchingResult>(
+      MatchingResult{time, pose_estimate, std::move(range_data_in_local),
+                     std::move(insertion_result)});
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>

cartographer/mapping_2d/local_trajectory_builder.h

@@ -45,6 +45,13 @@ class LocalTrajectoryBuilder {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
+  struct MatchingResult {
+    common::Time time;
+    transform::Rigid3d local_pose;
+    sensor::RangeData range_data_in_local;
+    // 'nullptr' if dropped by the motion filter.
+    std::unique_ptr<const InsertionResult> insertion_result;
+  };
 
   explicit LocalTrajectoryBuilder(
       const proto::LocalTrajectoryBuilderOptions& options);
@@ -55,14 +62,16 @@ class LocalTrajectoryBuilder {
 
   const mapping::PoseEstimate& pose_estimate() const;
 
-  // Range data must be approximately horizontal for 2D SLAM.
+  // Returns 'MatchingResult' when range data accumulation completed,
-  std::unique_ptr<InsertionResult> AddRangeData(
+  // otherwise 'nullptr'. Range data must be approximately horizontal
+  // for 2D SLAM.
+  std::unique_ptr<MatchingResult> AddRangeData(
       common::Time, const sensor::TimedRangeData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
   void AddOdometerData(const sensor::OdometryData& odometry_data);
 
  private:
-  std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
+  std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
       common::Time time, const sensor::RangeData& range_data);
   sensor::RangeData TransformAndFilterRangeData(
       const transform::Rigid3f& gravity_alignment,

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -58,7 +58,7 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
       options_.imu_gravity_time_constant(), imu_data);
 }
 
-std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 LocalTrajectoryBuilder::AddRangeData(const common::Time time,
                                      const sensor::TimedRangeData& range_data) {
   if (extrapolator_ == nullptr) {
@@ -106,7 +106,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
   return nullptr;
 }
 
-std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
     const common::Time time, const sensor::RangeData& range_data_in_tracking) {
   const sensor::RangeData filtered_range_data_in_tracking = {
@@ -166,10 +166,13 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       filtered_range_data_in_tracking, pose_estimate.cast<float>());
   last_pose_estimate_ = {time, pose_estimate,
                          filtered_range_data_in_local.returns};
-  return InsertIntoSubmap(
+  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
       time, filtered_range_data_in_local, filtered_range_data_in_tracking,
       high_resolution_point_cloud_in_tracking,
       low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
+  return common::make_unique<MatchingResult>(MatchingResult{
+      time, pose_estimate, std::move(filtered_range_data_in_local),
+      std::move(insertion_result)});
 }
 
 void LocalTrajectoryBuilder::AddOdometerData(
@@ -212,17 +215,17 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
               filtered_range_data_in_tracking.returns,
               transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
           options_.rotational_histogram_size());
-  return std::unique_ptr<InsertionResult>(new InsertionResult{
+  return common::make_unique<InsertionResult>(
-      std::make_shared<const mapping::TrajectoryNode::Data>(
+      InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>(
-          mapping::TrajectoryNode::Data{
+                          mapping::TrajectoryNode::Data{
-              time,
+                              time,
-              gravity_alignment,
+                              gravity_alignment,
-              {},  // 'filtered_point_cloud' is only used in 2D.
+                              {},  // 'filtered_point_cloud' is only used in 2D.
-              high_resolution_point_cloud_in_tracking,
+                              high_resolution_point_cloud_in_tracking,
-              low_resolution_point_cloud_in_tracking,
+                              low_resolution_point_cloud_in_tracking,
-              rotational_scan_matcher_histogram,
+                              rotational_scan_matcher_histogram,
-              pose_estimate}),
+                              pose_estimate}),
-      std::move(insertion_submaps)});
+                      std::move(insertion_submaps)});
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/local_trajectory_builder.h

@@ -44,6 +44,13 @@ class LocalTrajectoryBuilder {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
+  struct MatchingResult {
+    common::Time time;
+    transform::Rigid3d local_pose;
+    sensor::RangeData range_data_in_local;
+    // 'nullptr' if dropped by the motion filter.
+    std::unique_ptr<const InsertionResult> insertion_result;
+  };
 
   explicit LocalTrajectoryBuilder(
       const proto::LocalTrajectoryBuilderOptions& options);
@@ -53,13 +60,15 @@ class LocalTrajectoryBuilder {
   LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
 
   void AddImuData(const sensor::ImuData& imu_data);
-  std::unique_ptr<InsertionResult> AddRangeData(
+  // Returns 'MatchingResult' when range data accumulation completed,
+  // otherwise 'nullptr'.
+  std::unique_ptr<MatchingResult> AddRangeData(
       common::Time time, const sensor::TimedRangeData& range_data);
   void AddOdometerData(const sensor::OdometryData& odometry_data);
   const mapping::PoseEstimate& pose_estimate() const;
 
  private:
-  std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
+  std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
       common::Time time, const sensor::RangeData& range_data_in_tracking);
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(

cartographer/mapping_3d/local_trajectory_builder_test.cc

@@ -251,12 +251,13 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
     for (const TrajectoryNode& node : expected_trajectory) {
       AddLinearOnlyImuObservation(node.time, node.pose);
       const auto range_data = GenerateRangeData(node.pose);
-      if (local_trajectory_builder_->AddRangeData(
+      const std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
-              node.time,
+          matching_result = local_trajectory_builder_->AddRangeData(
-              sensor::TimedRangeData{
+              node.time, sensor::TimedRangeData{
-                  range_data.origin, range_data.returns, {}}) != nullptr) {
+                             range_data.origin, range_data.returns, {}});
-        const auto pose_estimate = local_trajectory_builder_->pose_estimate();
+      if (matching_result != nullptr) {
-        EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
+        EXPECT_THAT(matching_result->local_pose,
+                    transform::IsNearly(node.pose, 1e-1));
         ++num_poses;
         LOG(INFO) << "num_poses: " << num_poses;
       }