From 5396156968acb8a25f5c0a0a18c7020cd87fbb96 Mon Sep 17 00:00:00 2001 From: "Brandon D. Northcutt" Date: Mon, 3 Jul 2017 08:59:55 -0700 Subject: [PATCH] Submap serialization support. (#376) --- cartographer/mapping/proto/submap.proto | 30 +++++++++++++++++ cartographer/mapping/submaps.h | 7 ++-- cartographer/mapping_2d/submaps.cc | 18 ++++++++++- cartographer/mapping_2d/submaps.h | 6 +++- cartographer/mapping_2d/submaps_test.cc | 18 +++++++++++ cartographer/mapping_3d/submaps.cc | 22 ++++++++++++- cartographer/mapping_3d/submaps.h | 6 +++- cartographer/mapping_3d/submaps_test.cc | 43 +++++++++++++++++++++++++ 8 files changed, 142 insertions(+), 8 deletions(-) create mode 100644 cartographer/mapping/proto/submap.proto create mode 100644 cartographer/mapping_3d/submaps_test.cc diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto new file mode 100644 index 0000000..f95e363 --- /dev/null +++ b/cartographer/mapping/proto/submap.proto @@ -0,0 +1,30 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto2"; + +package cartographer.mapping.proto; + +import "cartographer/transform/proto/transform.proto"; +import "cartographer/mapping_2d/proto/probability_grid.proto"; +import "cartographer/mapping_3d/proto/hybrid_grid.proto"; + +message Submap { + optional transform.proto.Rigid3d local_pose = 1; + optional int32 num_range_data = 2; + optional bool finished = 3; + optional mapping_2d.proto.ProbabilityGrid probability_grid = 4; + optional mapping_3d.proto.HybridGrid high_resolution_hybrid_grid = 5; + optional mapping_3d.proto.HybridGrid low_resolution_hybrid_grid = 6; +} diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index 9797f25..b195ac6 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -72,14 +72,13 @@ class Submap { const transform::Rigid3d& global_submap_pose, proto::SubmapQuery::Response* response) const = 0; + protected: + void SetNumRangeData(int num_range_data) { num_range_data_ = num_range_data; } + private: const transform::Rigid3d local_pose_; - - protected: - // TODO(hrapp): This should be private. int num_range_data_ = 0; }; - } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index abe8354..e77b013 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -69,6 +69,22 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin) Eigen::Vector3d(origin.x(), origin.y(), 0.))), probability_grid_(limits) {} +Submap::Submap(const mapping::proto::Submap& proto) + : mapping::Submap(transform::ToRigid3(proto.local_pose())), + probability_grid_(ProbabilityGrid(proto.probability_grid())) { + SetNumRangeData(proto.num_range_data()); + finished_ = proto.finished(); +} + +mapping::proto::Submap Submap::ToProto() const { + mapping::proto::Submap proto; + *proto.mutable_local_pose() = transform::ToProto(local_pose()); + proto.set_num_range_data(num_range_data()); + proto.set_finished(finished_); + *proto.mutable_probability_grid() = probability_grid_.ToProto(); + return proto; +} + void Submap::ToResponseProto( const transform::Rigid3d&, mapping::proto::SubmapQuery::Response* const response) const { @@ -119,7 +135,7 @@ void Submap::InsertRangeData(const sensor::RangeData& range_data, const RangeDataInserter& range_data_inserter) { CHECK(!finished_); range_data_inserter.Insert(range_data, &probability_grid_); - ++num_range_data_; + SetNumRangeData(num_range_data() + 1); } void Submap::Finish() { diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index b4e093d..cf7dc33 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -22,6 +22,7 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/submap.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" @@ -44,9 +45,12 @@ proto::SubmapsOptions CreateSubmapsOptions( class Submap : public mapping::Submap { public: Submap(const MapLimits& limits, const Eigen::Vector2f& origin); + explicit Submap(const mapping::proto::Submap& proto); + + mapping::proto::Submap ToProto() const; const ProbabilityGrid& probability_grid() const { return probability_grid_; } - const bool finished() const { return finished_; } + bool finished() const { return finished_; } void ToResponseProto( const transform::Rigid3d& global_submap_pose, diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index feccbcd..a930b2f 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -68,6 +68,24 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { EXPECT_EQ(correct_num_scans, all_submaps.size() - 2); } +TEST(SubmapsTest, ToFromProto) { + Submap expected(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)), + Eigen::Vector2f(4.f, 5.f)); + const auto actual = Submap(expected.ToProto()); + EXPECT_TRUE(expected.local_pose().translation().isApprox( + actual.local_pose().translation(), 1e-6)); + EXPECT_TRUE(expected.local_pose().rotation().isApprox( + actual.local_pose().rotation(), 1e-6)); + EXPECT_EQ(expected.num_range_data(), actual.num_range_data()); + EXPECT_EQ(expected.finished(), actual.finished()); + EXPECT_NEAR(expected.probability_grid().limits().resolution(), + actual.probability_grid().limits().resolution(), 1e-6); + EXPECT_TRUE(expected.probability_grid().limits().max().isApprox( + actual.probability_grid().limits().max(), 1e-6)); + EXPECT_EQ(expected.probability_grid().limits().cell_limits().num_x_cells, + actual.probability_grid().limits().cell_limits().num_x_cells); +} + } // namespace } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 2192d64..b270111 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -326,6 +326,26 @@ Submap::Submap(const float high_resolution, const float low_resolution, high_resolution_hybrid_grid_(high_resolution), low_resolution_hybrid_grid_(low_resolution) {} +Submap::Submap(const mapping::proto::Submap& proto) + : mapping::Submap(transform::ToRigid3(proto.local_pose())), + high_resolution_hybrid_grid_(proto.high_resolution_hybrid_grid()), + low_resolution_hybrid_grid_(proto.low_resolution_hybrid_grid()) { + SetNumRangeData(proto.num_range_data()); + finished_ = proto.finished(); +} + +mapping::proto::Submap Submap::ToProto() const { + mapping::proto::Submap proto; + *proto.mutable_local_pose() = transform::ToProto(local_pose()); + proto.set_num_range_data(num_range_data()); + proto.set_finished(finished_); + *proto.mutable_high_resolution_hybrid_grid() = + mapping_3d::ToProto(high_resolution_hybrid_grid()); + *proto.mutable_low_resolution_hybrid_grid() = + mapping_3d::ToProto(low_resolution_hybrid_grid()); + return proto; +} + void Submap::ToResponseProto( const transform::Rigid3d& global_submap_pose, mapping::proto::SubmapQuery::Response* const response) const { @@ -372,7 +392,7 @@ void Submap::InsertRangeData(const sensor::RangeData& range_data, &high_resolution_hybrid_grid_); range_data_inserter.Insert(transformed_range_data, &low_resolution_hybrid_grid_); - ++num_range_data_; + SetNumRangeData(num_range_data() + 1); } void Submap::Finish() { diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index b7b2097..42ab9c8 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -24,6 +24,7 @@ #include "Eigen/Geometry" #include "cartographer/common/port.h" #include "cartographer/mapping/id.h" +#include "cartographer/mapping/proto/submap.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping_2d/probability_grid.h" @@ -51,6 +52,9 @@ class Submap : public mapping::Submap { public: Submap(float high_resolution, float low_resolution, const transform::Rigid3d& local_pose); + explicit Submap(const mapping::proto::Submap& proto); + + mapping::proto::Submap ToProto() const; const HybridGrid& high_resolution_hybrid_grid() const { return high_resolution_hybrid_grid_; @@ -58,7 +62,7 @@ class Submap : public mapping::Submap { const HybridGrid& low_resolution_hybrid_grid() const { return low_resolution_hybrid_grid_; } - const bool finished() const { return finished_; } + bool finished() const { return finished_; } void ToResponseProto( const transform::Rigid3d& global_submap_pose, diff --git a/cartographer/mapping_3d/submaps_test.cc b/cartographer/mapping_3d/submaps_test.cc new file mode 100644 index 0000000..ffdbc79 --- /dev/null +++ b/cartographer/mapping_3d/submaps_test.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping_3d/submaps.h" + +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_3d { +namespace { + +TEST(SubmapsTest, ToFromProto) { + const Submap expected(0.05, 0.25, + transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), + Eigen::Quaterniond(0., 0., 0., 1.))); + const auto actual = Submap(expected.ToProto()); + EXPECT_TRUE(expected.local_pose().translation().isApprox( + actual.local_pose().translation(), 1e-6)); + EXPECT_TRUE(expected.local_pose().rotation().isApprox( + actual.local_pose().rotation(), 1e-6)); + EXPECT_EQ(expected.num_range_data(), actual.num_range_data()); + EXPECT_EQ(expected.finished(), actual.finished()); + EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6); + EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6); +} + +} // namespace +} // namespace mapping_3d +} // namespace cartographer