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

Add parameter to Submap::ToProto() to determine whether to include loop (#821)

master
Christoph Schütte 2018-01-16 11:23:29 +01:00 committed by Wally B. Feed
parent 3660408ae6
commit 31ec89614c
8 changed files with 23 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/map_builder.cc
View File

@ -173,7 +173,8 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
submap_id_data.id.trajectory_id); submap_id_data.id.trajectory_id);
submap_proto->mutable_submap_id()->set_submap_index( submap_proto->mutable_submap_id()->set_submap_index(
submap_id_data.id.submap_index); submap_id_data.id.submap_index);
submap_id_data.data.submap->ToProto(submap_proto); submap_id_data.data.submap->ToProto(
submap_proto, true /* include_probability_grid_data */);
writer->WriteProto(proto); writer->WriteProto(proto);
} }
} }

3
cartographer/mapping/submaps.h
View File

@ -63,7 +63,8 @@ class Submap {
: local_pose_(local_submap_pose) {} : local_pose_(local_submap_pose) {}
virtual ~Submap() {} virtual ~Submap() {}
virtual void ToProto(proto::Submap* proto) const = 0; virtual void ToProto(proto::Submap* proto,
bool include_probability_grid_data) const = 0;
virtual void UpdateFromProto(const proto::Submap& proto) = 0; virtual void UpdateFromProto(const proto::Submap& proto) = 0;
// Pose of this submap in the local map frame. // Pose of this submap in the local map frame.

5
cartographer/mapping_2d/submaps.cc
View File

@ -74,13 +74,16 @@ Submap::Submap(const mapping::proto::Submap2D& proto)
finished_ = proto.finished(); finished_ = proto.finished();
} }
void Submap::ToProto(mapping::proto::Submap* const proto) const { void Submap::ToProto(mapping::proto::Submap* const proto,
bool include_probability_grid_data) const {
auto* const submap_2d = proto->mutable_submap_2d(); auto* const submap_2d = proto->mutable_submap_2d();
*submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); *submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
submap_2d->set_num_range_data(num_range_data()); submap_2d->set_num_range_data(num_range_data());
submap_2d->set_finished(finished_); submap_2d->set_finished(finished_);
if (include_probability_grid_data) {
*submap_2d->mutable_probability_grid() = probability_grid_.ToProto(); *submap_2d->mutable_probability_grid() = probability_grid_.ToProto();
} }
}
void Submap::UpdateFromProto(const mapping::proto::Submap& proto) { void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
CHECK(proto.has_submap_2d()); CHECK(proto.has_submap_2d());

3
cartographer/mapping_2d/submaps.h
View File

@ -47,7 +47,8 @@ class Submap : public mapping::Submap {
Submap(const MapLimits& limits, const Eigen::Vector2f& origin); Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
explicit Submap(const mapping::proto::Submap2D& proto); explicit Submap(const mapping::proto::Submap2D& proto);
void ToProto(mapping::proto::Submap* proto) const override; void ToProto(mapping::proto::Submap* proto,
bool include_probability_grid_data) const override;
void UpdateFromProto(const mapping::proto::Submap& proto) override; void UpdateFromProto(const mapping::proto::Submap& proto) override;
const ProbabilityGrid& probability_grid() const { return probability_grid_; } const ProbabilityGrid& probability_grid() const { return probability_grid_; }

2
cartographer/mapping_2d/submaps_test.cc
View File

@ -72,7 +72,7 @@ TEST(SubmapsTest, ToFromProto) {
Submap expected(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)), Submap expected(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)),
Eigen::Vector2f(4.f, 5.f)); Eigen::Vector2f(4.f, 5.f));
mapping::proto::Submap proto; mapping::proto::Submap proto;
expected.ToProto(&proto); expected.ToProto(&proto, true /* include_probability_grid_data */);
EXPECT_TRUE(proto.has_submap_2d()); EXPECT_TRUE(proto.has_submap_2d());
EXPECT_FALSE(proto.has_submap_3d()); EXPECT_FALSE(proto.has_submap_3d());
const auto actual = Submap(proto.submap_2d()); const auto actual = Submap(proto.submap_2d());

5
cartographer/mapping_3d/submaps.cc
View File

@ -215,16 +215,19 @@ Submap::Submap(const mapping::proto::Submap3D& proto)
finished_ = proto.finished(); finished_ = proto.finished();
} }
void Submap::ToProto(mapping::proto::Submap* const proto) const { void Submap::ToProto(mapping::proto::Submap* const proto,
bool include_probability_grid_data) const {
auto* const submap_3d = proto->mutable_submap_3d(); auto* const submap_3d = proto->mutable_submap_3d();
*submap_3d->mutable_local_pose() = transform::ToProto(local_pose()); *submap_3d->mutable_local_pose() = transform::ToProto(local_pose());
submap_3d->set_num_range_data(num_range_data()); submap_3d->set_num_range_data(num_range_data());
submap_3d->set_finished(finished_); submap_3d->set_finished(finished_);
if (include_probability_grid_data) {
*submap_3d->mutable_high_resolution_hybrid_grid() = *submap_3d->mutable_high_resolution_hybrid_grid() =
high_resolution_hybrid_grid().ToProto(); high_resolution_hybrid_grid().ToProto();
*submap_3d->mutable_low_resolution_hybrid_grid() = *submap_3d->mutable_low_resolution_hybrid_grid() =
low_resolution_hybrid_grid().ToProto(); low_resolution_hybrid_grid().ToProto();
} }
}
void Submap::UpdateFromProto(const mapping::proto::Submap& proto) { void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
CHECK(proto.has_submap_3d()); CHECK(proto.has_submap_3d());

3
cartographer/mapping_3d/submaps.h
View File

@ -47,7 +47,8 @@ class Submap : public mapping::Submap {
const transform::Rigid3d& local_submap_pose); const transform::Rigid3d& local_submap_pose);
explicit Submap(const mapping::proto::Submap3D& proto); explicit Submap(const mapping::proto::Submap3D& proto);
void ToProto(mapping::proto::Submap* proto) const override; void ToProto(mapping::proto::Submap* proto,
bool include_probability_grid_data) const override;
void UpdateFromProto(const mapping::proto::Submap& proto) override; void UpdateFromProto(const mapping::proto::Submap& proto) override;
const HybridGrid& high_resolution_hybrid_grid() const { const HybridGrid& high_resolution_hybrid_grid() const {

2
cartographer/mapping_3d/submaps_test.cc
View File

@ -28,7 +28,7 @@ TEST(SubmapsTest, ToFromProto) {
transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), transform::Rigid3d(Eigen::Vector3d(1., 2., 0.),
Eigen::Quaterniond(0., 0., 0., 1.))); Eigen::Quaterniond(0., 0., 0., 1.)));
mapping::proto::Submap proto; mapping::proto::Submap proto;
expected.ToProto(&proto); expected.ToProto(&proto, true /* include_probability_grid_data */);
EXPECT_FALSE(proto.has_submap_2d()); EXPECT_FALSE(proto.has_submap_2d());
EXPECT_TRUE(proto.has_submap_3d()); EXPECT_TRUE(proto.has_submap_3d());
const auto actual = Submap(proto.submap_3d()); const auto actual = Submap(proto.submap_3d());