From a4ff055d8f234a72d9af847a0512a5725cdf9b57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Mon, 3 Sep 2018 17:33:34 +0200 Subject: [PATCH] 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. --- .../io/internal/mapping_state_serialization.h | 2 + .../io/serialization_format_migration.cc | 65 ++++++++++++++++ .../io/serialization_format_migration.h | 9 +++ .../io/serialization_format_migration_test.cc | 77 ++++++++++++++++++- cartographer/mapping/3d/submap_3d.cc | 13 ++++ cartographer/mapping/3d/submap_3d.h | 4 + cartographer/mapping/map_builder.cc | 30 ++++++-- cartographer/mapping/proto/submap.proto | 1 + 8 files changed, 194 insertions(+), 7 deletions(-) diff --git a/cartographer/io/internal/mapping_state_serialization.h b/cartographer/io/internal/mapping_state_serialization.h index f4d7f42..2e7b7f3 100644 --- a/cartographer/io/internal/mapping_state_serialization.h +++ b/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( diff --git a/cartographer/io/serialization_format_migration.cc b/cartographer/io/serialization_format_migration.cc index 5f26710..c6fb7ce 100644 --- a/cartographer/io/serialization_format_migration.cc +++ b/cartographer/io/serialization_format_migration.cc @@ -19,6 +19,8 @@ #include #include +#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 +MigrateSubmapFormatVersion1ToVersion2( + const mapping::MapById& + submap_id_to_submap, + mapping::MapById& 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 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 diff --git a/cartographer/io/serialization_format_migration.h b/cartographer/io/serialization_format_migration.h index 20c6017..3570a8a 100644 --- a/cartographer/io/serialization_format_migration.h +++ b/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 +MigrateSubmapFormatVersion1ToVersion2( + const mapping::MapById& + submap_id_to_submaps, + mapping::MapById& node_id_to_nodes, + const mapping::proto::PoseGraph& pose_graph_proto); + } // namespace io } // namespace cartographer diff --git a/cartographer/io/serialization_format_migration_test.cc b/cartographer/io/serialization_format_migration_test.cc index 11bee22..24904ca 100644 --- a/cartographer/io/serialization_format_migration_test.cc +++ b/cartographer/io/serialization_format_migration_test.cc @@ -20,10 +20,13 @@ #include #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(reader_); AddLegacyDataToReader( 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 + 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 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 diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 36648d3..6e8fefe 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -20,6 +20,7 @@ #include #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(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( diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index c8a4a7c..5b70d57 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/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 high_resolution_hybrid_grid_; std::unique_ptr low_resolution_hybrid_grid_; + Eigen::VectorXf rotational_scan_matcher_histogram_; }; // The first active submap will be created on the insertion of the first range diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index ff340bb..0eeaeae 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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 MapBuilder::LoadState( transform::ToRigid3(landmark.global_pose())); } + MapById submap_id_to_submap; + MapById node_id_to_node; SerializedData proto; while (deserializer.ReadNextSerializedData(&proto)) { switch (proto.data_case()) { @@ -303,19 +306,20 @@ std::map 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()}); - pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); + proto.submap().submap_id().submap_index()}, + 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 = - node_poses.at(NodeId{proto.node().node_id().trajectory_id(), - proto.node().node_id().node_index()}); + const NodeId node_id(proto.node().node_id().trajectory_id(), + 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 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. diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index 6fab2c4..72b4e62 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/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; }