From 818e5e1a4400c3b5131cccbcbc03f926fb5cc6b7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Tue, 14 Nov 2017 17:19:14 +0100
Subject: [PATCH] Add local SLAM result callback. (#574)

Depends on ~~#619~~ (merged) and ~~#617~~ (merged).
Related to #508.

Also, if cartographer_ros is going to use this, and we wish to serialize the saved range data, that will have to be handled in cartographer_ros, right?
---
 cartographer/mapping/global_trajectory_builder.h | 16 ++++++++++++----
 .../global_trajectory_builder_interface.h        |  9 +++++++++
 cartographer/mapping/map_builder.cc              | 14 ++++++++++----
 cartographer/mapping/map_builder.h               |  8 +++++++-
 4 files changed, 38 insertions(+), 9 deletions(-)

diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h
index 7031d15..ec04469 100644
--- a/cartographer/mapping/global_trajectory_builder.h
+++ b/cartographer/mapping/global_trajectory_builder.h
@@ -29,12 +29,14 @@ template <typename LocalTrajectoryBuilder,
 class GlobalTrajectoryBuilder
     : public mapping::GlobalTrajectoryBuilderInterface {
  public:
-  GlobalTrajectoryBuilder(const LocalTrajectoryBuilderOptions& options,
-                          const int trajectory_id,
-                          SparsePoseGraph* const sparse_pose_graph)
+  GlobalTrajectoryBuilder(
+      const LocalTrajectoryBuilderOptions& options, const int trajectory_id,
+      SparsePoseGraph* const sparse_pose_graph,
+      const LocalSlamResultCallback& local_slam_result_callback)
       : trajectory_id_(trajectory_id),
         sparse_pose_graph_(sparse_pose_graph),
-        local_trajectory_builder_(options) {}
+        local_trajectory_builder_(options),
+        local_slam_result_callback_(local_slam_result_callback) {}
   ~GlobalTrajectoryBuilder() override {}
 
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
@@ -62,6 +64,11 @@ class GlobalTrajectoryBuilder
               matching_result->insertion_result->insertion_submaps));
       CHECK_EQ(node_id->trajectory_id, trajectory_id_);
     }
+    if (local_slam_result_callback_) {
+      local_slam_result_callback_(
+          trajectory_id_, matching_result->time, matching_result->local_pose,
+          std::move(matching_result->range_data_in_local), std::move(node_id));
+    }
   }
 
   void AddSensorData(const sensor::ImuData& imu_data) override {
@@ -85,6 +92,7 @@ class GlobalTrajectoryBuilder
   const int trajectory_id_;
   SparsePoseGraph* const sparse_pose_graph_;
   LocalTrajectoryBuilder local_trajectory_builder_;
+  LocalSlamResultCallback local_slam_result_callback_;
 };
 
 }  // namespace mapping
diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h
index 6c4cd26..04e85c1 100644
--- a/cartographer/mapping/global_trajectory_builder_interface.h
+++ b/cartographer/mapping/global_trajectory_builder_interface.h
@@ -58,6 +58,15 @@ class GlobalTrajectoryBuilderInterface {
   virtual void AddSensorData(const sensor::OdometryData& odometry_data) = 0;
   virtual void AddSensorData(
       const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
+
+  // A callback which is called after local SLAM processes an accumulated
+  // 'sensor::RangeData'. If the data was inserted into a submap, reports the
+  // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
+  using LocalSlamResultCallback =
+      std::function<void(int /* trajectory ID */, common::Time,
+                         transform::Rigid3d /* local pose estimate */,
+                         sensor::RangeData /* in local frame */,
+                         std::unique_ptr<const mapping::NodeId>)>;
 };
 
 }  // namespace mapping
diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index 975d82f..50a1d0c 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -53,8 +53,12 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
   return options;
 }
 
-MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
-    : options_(options), thread_pool_(options.num_background_threads()) {
+MapBuilder::MapBuilder(
+    const proto::MapBuilderOptions& options,
+    const LocalSlamResultCallback& local_slam_result_callback)
+    : options_(options),
+      thread_pool_(options.num_background_threads()),
+      local_slam_result_callback_(local_slam_result_callback) {
   if (options.use_trajectory_builder_2d()) {
     sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
         options_.sparse_pose_graph_options(), &thread_pool_);
@@ -83,7 +87,8 @@ int MapBuilder::AddTrajectoryBuilder(
                 mapping_3d::proto::LocalTrajectoryBuilderOptions,
                 mapping_3d::SparsePoseGraph>>(
                 trajectory_options.trajectory_builder_3d_options(),
-                trajectory_id, sparse_pose_graph_3d_.get())));
+                trajectory_id, sparse_pose_graph_3d_.get(),
+                local_slam_result_callback_)));
   } else {
     CHECK(trajectory_options.has_trajectory_builder_2d_options());
     trajectory_builders_.push_back(
@@ -94,7 +99,8 @@ int MapBuilder::AddTrajectoryBuilder(
                 mapping_2d::proto::LocalTrajectoryBuilderOptions,
                 mapping_2d::SparsePoseGraph>>(
                 trajectory_options.trajectory_builder_2d_options(),
-                trajectory_id, sparse_pose_graph_2d_.get())));
+                trajectory_id, sparse_pose_graph_2d_.get(),
+                local_slam_result_callback_)));
   }
   if (trajectory_options.pure_localization()) {
     constexpr int kSubmapsToKeep = 3;
diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h
index ed97812..9f90e49 100644
--- a/cartographer/mapping/map_builder.h
+++ b/cartographer/mapping/map_builder.h
@@ -49,7 +49,11 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
 // and a SparsePoseGraph for loop closure.
 class MapBuilder {
  public:
-  MapBuilder(const proto::MapBuilderOptions& options);
+  using LocalSlamResultCallback =
+      GlobalTrajectoryBuilderInterface::LocalSlamResultCallback;
+
+  MapBuilder(const proto::MapBuilderOptions& options,
+             const LocalSlamResultCallback& local_slam_result_callback);
   ~MapBuilder();
 
   MapBuilder(const MapBuilder&) = delete;
@@ -101,6 +105,8 @@ class MapBuilder {
   std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
   mapping::SparsePoseGraph* sparse_pose_graph_;
 
+  LocalSlamResultCallback local_slam_result_callback_;
+
   sensor::Collator sensor_collator_;
   std::vector<std::unique_ptr<mapping::TrajectoryBuilder>> trajectory_builders_;
 };