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; }