Store histogram in submaps: prepare for backward compatiblity (#1405)

This makes the map builder backwards compatible to the current map pbstream (version 1). The PR prepares for #1277, where pbstream version 2 will be introduced. Backwards compatibility was discussed in #1277.

When a map with pbstream version 1 is loaded, a rotational scan matcher histogram is generated for each submap using the histograms of all nodes that were inserted to the submap during local SLAM. Once this backwards compatibility is in place, I would like to introduce the new format with #1277.

cartographer/io/internal/mapping_state_serialization.h

@@ -26,6 +26,8 @@ namespace io {
 
 // The current serialization format version.
 static constexpr int kMappingStateSerializationFormatVersion = 1;
+static constexpr int kFormatVersionWithoutSubmapHistograms =
+    kMappingStateSerializationFormatVersion;
 
 // Serialize mapping state to a pbstream.
 void WritePbStream(

cartographer/io/serialization_format_migration.cc

@@ -19,6 +19,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include "cartographer/mapping/3d/submap_3d.h"
+#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h"
 #include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h"
 #include "cartographer/mapping/proto/internal/legacy_submap.pb.h"
@@ -295,5 +297,68 @@ void MigrateStreamFormatToVersion1(
                             output);
 }
 
+mapping::MapById<mapping::SubmapId, mapping::proto::Submap>
+MigrateSubmapFormatVersion1ToVersion2(
+    const mapping::MapById<mapping::SubmapId, mapping::proto::Submap>&
+        submap_id_to_submap,
+    mapping::MapById<mapping::NodeId, mapping::proto::Node>& node_id_to_node,
+    const mapping::proto::PoseGraph& pose_graph_proto) {
+  using namespace mapping;
+  if (submap_id_to_submap.empty() ||
+      submap_id_to_submap.begin()->data.has_submap_2d()) {
+    return submap_id_to_submap;
+  }
+
+  MapById<SubmapId, proto::Submap> migrated_submaps = submap_id_to_submap;
+  for (const proto::PoseGraph::Constraint& constraint_proto :
+       pose_graph_proto.constraint()) {
+    if (constraint_proto.tag() == proto::PoseGraph::Constraint::INTRA_SUBMAP) {
+      NodeId node_id{constraint_proto.node_id().trajectory_id(),
+                     constraint_proto.node_id().node_index()};
+      CHECK(node_id_to_node.Contains(node_id));
+      const TrajectoryNode::Data node_data =
+          FromProto(node_id_to_node.at(node_id).node_data());
+      const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity =
+          node_data.rotational_scan_matcher_histogram;
+
+      SubmapId submap_id{constraint_proto.submap_id().trajectory_id(),
+                         constraint_proto.submap_id().submap_index()};
+      CHECK(migrated_submaps.Contains(submap_id));
+      proto::Submap& migrated_submap_proto = migrated_submaps.at(submap_id);
+      CHECK(migrated_submap_proto.has_submap_3d());
+
+      proto::Submap3D* submap_3d_proto =
+          migrated_submap_proto.mutable_submap_3d();
+      const double submap_yaw_from_gravity =
+          transform::GetYaw(transform::ToRigid3(submap_3d_proto->local_pose())
+                                .inverse()
+                                .rotation() *
+                            node_data.local_pose.rotation() *
+                            node_data.gravity_alignment.inverse());
+      const Eigen::VectorXf rotational_scan_matcher_histogram_in_submap =
+          scan_matching::RotationalScanMatcher::RotateHistogram(
+              rotational_scan_matcher_histogram_in_gravity,
+              submap_yaw_from_gravity);
+
+      if (submap_3d_proto->rotational_scan_matcher_histogram_size() == 0) {
+        for (Eigen::VectorXf::Index i = 0;
+             i != rotational_scan_matcher_histogram_in_submap.size(); ++i) {
+          submap_3d_proto->add_rotational_scan_matcher_histogram(
+              rotational_scan_matcher_histogram_in_submap(i));
+        }
+      } else {
+        auto submap_histogram =
+            submap_3d_proto->mutable_rotational_scan_matcher_histogram();
+        for (Eigen::VectorXf::Index i = 0;
+             i != rotational_scan_matcher_histogram_in_submap.size(); ++i) {
+          *submap_histogram->Mutable(i) +=
+              rotational_scan_matcher_histogram_in_submap(i);
+        }
+      }
+    }
+  }
+  return migrated_submaps;
+}
+
 }  // namespace io
 }  // namespace cartographer

cartographer/io/serialization_format_migration.h

@@ -18,6 +18,8 @@
 #define CARTOGRAPHER_IO_SERIALIZATION_FORMAT_MIGRATION_H_
 
 #include "cartographer/io/proto_stream_interface.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
 
 namespace cartographer {
 namespace io {
@@ -31,6 +33,13 @@ void MigrateStreamFormatToVersion1(
     cartographer::io::ProtoStreamWriterInterface* const output,
     bool migrate_grid_format);
 
+mapping::MapById<mapping::SubmapId, mapping::proto::Submap>
+MigrateSubmapFormatVersion1ToVersion2(
+    const mapping::MapById<mapping::SubmapId, mapping::proto::Submap>&
+        submap_id_to_submaps,
+    mapping::MapById<mapping::NodeId, mapping::proto::Node>& node_id_to_nodes,
+    const mapping::proto::PoseGraph& pose_graph_proto);
+
 }  // namespace io
 }  // namespace cartographer
 

cartographer/io/serialization_format_migration_test.cc

@@ -20,10 +20,13 @@
 #include <memory>
 
 #include "cartographer/io/internal/in_memory_proto_stream.h"
+#include "cartographer/mapping/internal/testing/test_helpers.h"
 #include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h"
 #include "cartographer/mapping/proto/pose_graph.pb.h"
 #include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+#include "cartographer/mapping/trajectory_node.h"
+#include "cartographer/transform/transform.h"
 #include "gmock/gmock.h"
 #include "google/protobuf/text_format.h"
 #include "gtest/gtest.h"
@@ -69,7 +72,7 @@ class MigrationTest : public ::testing::Test {
   }
 
  protected:
-  void SetUp() {
+  void SetUp() override {
     AddLegacyDataToReader<mapping::proto::LegacySerializedData>(reader_);
     AddLegacyDataToReader<mapping::proto::LegacySerializedDataLegacySubmap>(
         reader_for_migrating_grid_);
@@ -100,6 +103,44 @@ class MigrationTest : public ::testing::Test {
   static constexpr int kNumOriginalMessages = 9;
 };
 
+class SubmapHistogramMigrationTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    CreateSubmap();
+    CreateNode();
+    CreatePoseGraphWithNodeToSubmapConstraint();
+  }
+
+ private:
+  void CreateSubmap() {
+    submap_ = mapping::testing::CreateFakeSubmap3D();
+    submap_.mutable_submap_3d()->clear_rotational_scan_matcher_histogram();
+  }
+
+  void CreateNode() {
+    node_ = mapping::testing::CreateFakeNode();
+    constexpr int histogram_size = 10;
+    for (int i = 0; i < histogram_size; ++i) {
+      node_.mutable_node_data()->add_rotational_scan_matcher_histogram(1.f);
+    }
+  }
+
+  void CreatePoseGraphWithNodeToSubmapConstraint() {
+    mapping::proto::PoseGraph::Constraint* constraint =
+        pose_graph_.add_constraint();
+    constraint->set_tag(mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP);
+    *constraint->mutable_node_id() = node_.node_id();
+    *constraint->mutable_submap_id() = submap_.submap_id();
+    *constraint->mutable_relative_pose() =
+        transform::ToProto(transform::Rigid3d::Identity());
+  }
+
+ protected:
+  mapping::proto::PoseGraph pose_graph_;
+  mapping::proto::Submap submap_;
+  mapping::proto::Node node_;
+};
+
 TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) {
   MigrateStreamFormatToVersion1(&reader_, writer_.get(),
                                 false /* migrate_grid_format */);
@@ -175,6 +216,40 @@ TEST_F(MigrationTest, SerializedDataOrderAfterGridMigrationIsCorrect) {
   EXPECT_TRUE(serialized[8].has_landmark_data());
 }
 
+TEST_F(SubmapHistogramMigrationTest,
+       SubmapHistogramGenerationFromTrajectoryNodes) {
+  mapping::MapById<mapping::SubmapId, mapping::proto::Submap>
+      submap_id_to_submap;
+  mapping::SubmapId submap_id(submap_.submap_id().trajectory_id(),
+                              submap_.submap_id().submap_index());
+  submap_id_to_submap.Insert(submap_id, submap_);
+
+  mapping::MapById<mapping::NodeId, mapping::proto::Node> node_id_to_node;
+  mapping::NodeId node_id(node_.node_id().trajectory_id(),
+                          node_.node_id().node_index());
+  node_id_to_node.Insert(node_id, node_);
+
+  const auto submap_id_to_submap_migrated =
+      MigrateSubmapFormatVersion1ToVersion2(submap_id_to_submap,
+                                            node_id_to_node, pose_graph_);
+
+  EXPECT_EQ(submap_id_to_submap_migrated.size(), submap_id_to_submap.size());
+  const mapping::proto::Submap& migrated_submap =
+      submap_id_to_submap_migrated.at(submap_id);
+
+  EXPECT_FALSE(migrated_submap.has_submap_2d());
+  EXPECT_TRUE(migrated_submap.has_submap_3d());
+  const mapping::proto::Submap3D& migrated_submap_3d =
+      migrated_submap.submap_3d();
+  const mapping::proto::TrajectoryNodeData& node_data = node_.node_data();
+  EXPECT_EQ(migrated_submap_3d.rotational_scan_matcher_histogram_size(),
+            node_data.rotational_scan_matcher_histogram_size());
+  for (int i = 0; i < node_data.rotational_scan_matcher_histogram_size(); ++i) {
+    EXPECT_EQ(migrated_submap_3d.rotational_scan_matcher_histogram(i),
+              node_data.rotational_scan_matcher_histogram(i));
+  }
+}
+
 }  // namespace
 }  // namespace io
 }  // namespace cartographer

cartographer/mapping/3d/submap_3d.cc

@@ -20,6 +20,7 @@
 #include <limits>
 
 #include "cartographer/common/math.h"
+#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h"
 #include "cartographer/sensor/range_data.h"
 #include "glog/logging.h"
 
@@ -221,6 +222,11 @@ proto::Submap Submap3D::ToProto(
     *submap_3d->mutable_low_resolution_hybrid_grid() =
         low_resolution_hybrid_grid().ToProto();
   }
+  for (Eigen::VectorXf::Index i = 0;
+       i != rotational_scan_matcher_histogram_.size(); ++i) {
+    submap_3d->add_rotational_scan_matcher_histogram(
+        rotational_scan_matcher_histogram_(i));
+  }
   return proto;
 }
 
@@ -240,6 +246,13 @@ void Submap3D::UpdateFromProto(const proto::Submap3D& submap_3d) {
     low_resolution_hybrid_grid_ =
         absl::make_unique<HybridGrid>(submap_3d.low_resolution_hybrid_grid());
   }
+  rotational_scan_matcher_histogram_ =
+      Eigen::VectorXf::Zero(submap_3d.rotational_scan_matcher_histogram_size());
+  for (Eigen::VectorXf::Index i = 0;
+       i != submap_3d.rotational_scan_matcher_histogram_size(); ++i) {
+    rotational_scan_matcher_histogram_(i) =
+        submap_3d.rotational_scan_matcher_histogram(i);
+  }
 }
 
 void Submap3D::ToResponseProto(

cartographer/mapping/3d/submap_3d.h

@@ -58,6 +58,9 @@ class Submap3D : public Submap {
   const HybridGrid& low_resolution_hybrid_grid() const {
     return *low_resolution_hybrid_grid_;
   }
+  const Eigen::VectorXf& rotational_scan_matcher_histogram() const {
+    return rotational_scan_matcher_histogram_;
+  }
 
   // Insert 'range_data' into this submap using 'range_data_inserter'. The
   // submap must not be finished yet.
@@ -71,6 +74,7 @@ class Submap3D : public Submap {
 
   std::unique_ptr<HybridGrid> high_resolution_hybrid_grid_;
   std::unique_ptr<HybridGrid> low_resolution_hybrid_grid_;
+  Eigen::VectorXf rotational_scan_matcher_histogram_;
 };
 
 // The first active submap will be created on the insertion of the first range

cartographer/mapping/map_builder.cc

@@ -21,6 +21,7 @@
 #include "cartographer/io/internal/mapping_state_serialization.h"
 #include "cartographer/io/proto_stream.h"
 #include "cartographer/io/proto_stream_deserializer.h"
+#include "cartographer/io/serialization_format_migration.h"
 #include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
 #include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
 #include "cartographer/mapping/internal/2d/pose_graph_2d.h"
@@ -287,6 +288,8 @@ std::map<int, int> MapBuilder::LoadState(
                                  transform::ToRigid3(landmark.global_pose()));
   }
 
+  MapById<SubmapId, mapping::proto::Submap> submap_id_to_submap;
+  MapById<NodeId, mapping::proto::Node> node_id_to_node;
   SerializedData proto;
   while (deserializer.ReadNextSerializedData(&proto)) {
     switch (proto.data_case()) {
@@ -303,19 +306,20 @@ std::map<int, int> MapBuilder::LoadState(
         proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
             trajectory_remapping.at(
                 proto.submap().submap_id().trajectory_id()));
-        const transform::Rigid3d& submap_pose = submap_poses.at(
+        submap_id_to_submap.Insert(
             SubmapId{proto.submap().submap_id().trajectory_id(),
-                     proto.submap().submap_id().submap_index()});
+                     proto.submap().submap_id().submap_index()},
-        pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
+            proto.submap());
         break;
       }
       case SerializedData::kNode: {
         proto.mutable_node()->mutable_node_id()->set_trajectory_id(
             trajectory_remapping.at(proto.node().node_id().trajectory_id()));
-        const transform::Rigid3d& node_pose =
+        const NodeId node_id(proto.node().node_id().trajectory_id(),
-            node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
+                             proto.node().node_id().node_index());
-                                 proto.node().node_id().node_index()});
+        const transform::Rigid3d& node_pose = node_poses.at(node_id);
         pose_graph_->AddNodeFromProto(node_pose, proto.node());
+        node_id_to_node.Insert(node_id, proto.node());
         break;
       }
       case SerializedData::kTrajectoryData: {
@@ -360,6 +364,20 @@ std::map<int, int> MapBuilder::LoadState(
     }
   }
 
+  // TODO(schwoere): Remove backwards compatibility once the pbstream format
+  // version 2 is established.
+  if (deserializer.header().format_version() ==
+      io::kFormatVersionWithoutSubmapHistograms) {
+    submap_id_to_submap =
+        cartographer::io::MigrateSubmapFormatVersion1ToVersion2(
+            submap_id_to_submap, node_id_to_node, pose_graph_proto);
+  }
+
+  for (const auto& submap_id_submap : submap_id_to_submap) {
+    pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id_submap.id),
+                                    submap_id_submap.data);
+  }
+
   if (load_frozen_state) {
     // Add information about which nodes belong to which submap.
     // Required for 3D pure localization.

cartographer/mapping/proto/submap.proto

@@ -35,4 +35,5 @@ message Submap3D {
   bool finished = 3;
   HybridGrid high_resolution_hybrid_grid = 4;
   HybridGrid low_resolution_hybrid_grid = 5;
+  repeated float rotational_scan_matcher_histogram = 6;
 }