From 404bbfcc2bf48fe1a5e85d0b20d6119032fe204a Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Thu, 27 Oct 2016 16:20:58 +0200
Subject: [PATCH] Add SubmapToProto to the map builder. (#110)

---
 cartographer/mapping/CMakeLists.txt |  1 +
 cartographer/mapping/map_builder.cc | 27 +++++++++++++++++++++++++++
 cartographer/mapping/map_builder.h  |  7 +++++++
 3 files changed, 35 insertions(+)

diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt
index 089f9bf..9d7fa95 100644
--- a/cartographer/mapping/CMakeLists.txt
+++ b/cartographer/mapping/CMakeLists.txt
@@ -76,6 +76,7 @@ google_library(mapping_map_builder
     mapping_3d_sparse_pose_graph
     mapping_collated_trajectory_builder
     mapping_proto_map_builder_options
+    mapping_proto_submap_visualization
     mapping_sparse_pose_graph
     mapping_submaps
     mapping_trajectory_builder
diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index 76a839a..91c3e36 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -119,6 +119,33 @@ proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() {
                  trajectory_ids_);
 }
 
+string MapBuilder::SubmapToProto(const int trajectory_id,
+                                 const int submap_index,
+                                 proto::SubmapQuery::Response* const response) {
+  if (trajectory_id < 0 || trajectory_id >= num_trajectory_builders()) {
+    return "Requested submap from trajectory " + std::to_string(trajectory_id) +
+           " but there are only " + std::to_string(num_trajectory_builders()) +
+           " trajectories.";
+  }
+
+  const Submaps* const submaps =
+      trajectory_builders_.at(trajectory_id)->submaps();
+  if (submap_index < 0 || submap_index >= submaps->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()) +
+           " submaps in this trajectory.";
+  }
+
+  response->set_submap_version(submaps->Get(submap_index)->end_laser_fan_index);
+  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 "";
+}
+
 int MapBuilder::num_trajectory_builders() const {
   return trajectory_builders_.size();
 }
diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h
index f59a6e2..2f87ee3 100644
--- a/cartographer/mapping/map_builder.h
+++ b/cartographer/mapping/map_builder.h
@@ -29,6 +29,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
+#include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_builder.h"
@@ -72,6 +73,12 @@ class MapBuilder {
   // Returns the trajectory connectivity.
   proto::TrajectoryConnectivity GetTrajectoryConnectivity();
 
+  // Fills the SubmapQuery::Response corresponding to 'submap_index' from
+  // 'trajectory_id'. Returns an error string on failure, or an empty string on
+  // success.
+  string SubmapToProto(int trajectory_id, int submap_index,
+                       proto::SubmapQuery::Response* response);
+
   int num_trajectory_builders() const;
 
   mapping::SparsePoseGraph* sparse_pose_graph();