diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 1ff497c..5c550db 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -135,7 +135,24 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id, void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { // We serialize the pose graph followed by all the data referenced in it. writer->WriteProto(sparse_pose_graph_->ToProto()); - // TODO(whess): Serialize submaps. + // Next we serialize all submap data. + const auto submap_data = sparse_pose_graph_->GetAllSubmapData(); + for (int trajectory_id = 0; + trajectory_id != static_cast(submap_data.size()); ++trajectory_id) { + for (int submap_index = 0; + submap_index != static_cast(submap_data[trajectory_id].size()); + ++submap_index) { + proto::SerializedData proto; + auto* const submap_proto = proto.mutable_submap(); + // TODO(whess): Handle trimmed data. + submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id); + submap_proto->mutable_submap_id()->set_submap_index(submap_index); + submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto); + // TODO(whess): Only enable optionally? Resulting pbstream files will be + // a lot larger now. + writer->WriteProto(proto); + } + } // TODO(whess): Serialize range data ("scan") for each trajectory node. // TODO(whess): Serialize additional sensor data: IMU, odometry. } diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto new file mode 100644 index 0000000..480b7ae --- /dev/null +++ b/cartographer/mapping/proto/serialization.proto @@ -0,0 +1,31 @@ +// 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/mapping/proto/sparse_pose_graph.proto"; +import "cartographer/mapping/proto/submap.proto"; + +message Submap { + optional SubmapId submap_id = 1; + optional Submap2D submap_2d = 2; + optional Submap3D submap_3d = 3; +} + +message SerializedData { + optional Submap submap = 1; + // TODO(whess): Add range data, IMU data, odometry. +} diff --git a/cartographer/mapping/proto/sparse_pose_graph.proto b/cartographer/mapping/proto/sparse_pose_graph.proto index 64f043f..c2ac704 100644 --- a/cartographer/mapping/proto/sparse_pose_graph.proto +++ b/cartographer/mapping/proto/sparse_pose_graph.proto @@ -19,18 +19,18 @@ package cartographer.mapping.proto; import "cartographer/mapping/proto/trajectory.proto"; import "cartographer/transform/proto/transform.proto"; +message SubmapId { + optional int32 trajectory_id = 1; + optional int32 submap_index = 2; // Submap index in the given trajectory. +} + +message ScanId { + optional int32 trajectory_id = 1; + optional int32 scan_index = 2; // Scan index in the given trajectory. +} + message SparsePoseGraph { message Constraint { - message SubmapId { - optional int32 trajectory_id = 1; - optional int32 submap_index = 2; // Submap index in the given trajectory. - } - - message ScanId { - optional int32 trajectory_id = 1; - optional int32 scan_index = 2; // Scan index in the given trajectory. - } - // Differentiates between intra-submap (where the scan was inserted into the // submap) and inter-submap constraints (where the scan was not inserted // into the submap). diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index f95e363..184a04e 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/cartographer/mapping/proto/submap.proto @@ -16,15 +16,23 @@ 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"; +import "cartographer/transform/proto/transform.proto"; -message Submap { +// Serialized state of a mapping_2d::Submap. +message Submap2D { 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; +} + +// Serialized state of a mapping_3d::Submap. +message Submap3D { + optional transform.proto.Rigid3d local_pose = 1; + optional int32 num_range_data = 2; + optional bool finished = 3; + optional mapping_3d.proto.HybridGrid high_resolution_hybrid_grid = 4; + optional mapping_3d.proto.HybridGrid low_resolution_hybrid_grid = 5; } diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index b195ac6..c6b9c4c 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -25,6 +25,7 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/probability_grid.h" @@ -61,6 +62,8 @@ class Submap { Submap(const transform::Rigid3d& local_pose) : local_pose_(local_pose) {} virtual ~Submap() {} + virtual void ToProto(proto::Submap* proto) const = 0; + // Local SLAM pose of this submap. transform::Rigid3d local_pose() const { return local_pose_; } @@ -73,7 +76,9 @@ class Submap { proto::SubmapQuery::Response* response) const = 0; protected: - void SetNumRangeData(int num_range_data) { num_range_data_ = num_range_data; } + void SetNumRangeData(const int num_range_data) { + num_range_data_ = num_range_data; + } private: const transform::Rigid3d local_pose_; diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index e77b013..d727906 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -69,20 +69,19 @@ 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) +Submap::Submap(const mapping::proto::Submap2D& 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::ToProto(mapping::proto::Submap* const proto) const { + auto* const submap_2d = proto->mutable_submap_2d(); + *submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); + submap_2d->set_num_range_data(num_range_data()); + submap_2d->set_finished(finished_); + *submap_2d->mutable_probability_grid() = probability_grid_.ToProto(); } void Submap::ToResponseProto( diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index cf7dc33..ab6f6b9 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -22,7 +22,7 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/proto/submap.pb.h" +#include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" @@ -45,9 +45,9 @@ proto::SubmapsOptions CreateSubmapsOptions( class Submap : public mapping::Submap { public: Submap(const MapLimits& limits, const Eigen::Vector2f& origin); - explicit Submap(const mapping::proto::Submap& proto); + explicit Submap(const mapping::proto::Submap2D& proto); - mapping::proto::Submap ToProto() const; + void ToProto(mapping::proto::Submap* proto) const override; const ProbabilityGrid& probability_grid() const { return probability_grid_; } bool finished() const { return finished_; } diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index a930b2f..16b0628 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -71,7 +71,11 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { 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()); + mapping::proto::Submap proto; + expected.ToProto(&proto); + EXPECT_TRUE(proto.has_submap_2d()); + EXPECT_FALSE(proto.has_submap_3d()); + const auto actual = Submap(proto.submap_2d()); EXPECT_TRUE(expected.local_pose().translation().isApprox( actual.local_pose().translation(), 1e-6)); EXPECT_TRUE(expected.local_pose().rotation().isApprox( diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index b270111..2485a19 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -326,7 +326,7 @@ 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) +Submap::Submap(const mapping::proto::Submap3D& 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()) { @@ -334,16 +334,15 @@ Submap::Submap(const mapping::proto::Submap& proto) 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() = +void Submap::ToProto(mapping::proto::Submap* const proto) const { + auto* const submap_3d = proto->mutable_submap_3d(); + *submap_3d->mutable_local_pose() = transform::ToProto(local_pose()); + submap_3d->set_num_range_data(num_range_data()); + submap_3d->set_finished(finished_); + *submap_3d->mutable_high_resolution_hybrid_grid() = mapping_3d::ToProto(high_resolution_hybrid_grid()); - *proto.mutable_low_resolution_hybrid_grid() = + *submap_3d->mutable_low_resolution_hybrid_grid() = mapping_3d::ToProto(low_resolution_hybrid_grid()); - return proto; } void Submap::ToResponseProto( diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 42ab9c8..2d4a9e2 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -24,7 +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/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping_2d/probability_grid.h" @@ -52,9 +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); + explicit Submap(const mapping::proto::Submap3D& proto); - mapping::proto::Submap ToProto() const; + void ToProto(mapping::proto::Submap* proto) const override; const HybridGrid& high_resolution_hybrid_grid() const { return high_resolution_hybrid_grid_; diff --git a/cartographer/mapping_3d/submaps_test.cc b/cartographer/mapping_3d/submaps_test.cc index ffdbc79..bb48762 100644 --- a/cartographer/mapping_3d/submaps_test.cc +++ b/cartographer/mapping_3d/submaps_test.cc @@ -27,7 +27,11 @@ 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()); + mapping::proto::Submap proto; + expected.ToProto(&proto); + EXPECT_FALSE(proto.has_submap_2d()); + EXPECT_TRUE(proto.has_submap_3d()); + const auto actual = Submap(proto.submap_3d()); EXPECT_TRUE(expected.local_pose().translation().isApprox( actual.local_pose().translation(), 1e-6)); EXPECT_TRUE(expected.local_pose().rotation().isApprox(