12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Martin Schwörer 2018-09-03 17:33:34 +02:00 committed by Wally B. Feed
parent 4c2104473c
commit a4ff055d8f
8 changed files with 194 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/io/internal/mapping_state_serialization.h
View File

@ -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(

65
cartographer/io/serialization_format_migration.cc
View File

@ -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

9
cartographer/io/serialization_format_migration.h
View File

@ -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

77
cartographer/io/serialization_format_migration_test.cc
View File

@ -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

13
cartographer/mapping/3d/submap_3d.cc
View File

@ -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(

4
cartographer/mapping/3d/submap_3d.h
View File

@ -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

30
cartographer/mapping/map_builder.cc
View File

@ -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()});
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<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.

1
cartographer/mapping/proto/submap.proto
View File

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