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

Serialize submaps. (#380)

master
Wolfgang Hess 2017-07-04 12:11:54 +02:00 committed by GitHub
parent 5396156968
commit 31f77a01d2
11 changed files with 108 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cartographer/mapping/map_builder.cc
View File

@ -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<int>(submap_data.size()); ++trajectory_id) {
for (int submap_index = 0;
submap_index != static_cast<int>(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.
}

31
cartographer/mapping/proto/serialization.proto Normal file
View File

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

12
cartographer/mapping/proto/sparse_pose_graph.proto
View File

@ -19,18 +19,18 @@ package cartographer.mapping.proto;
import "cartographer/mapping/proto/trajectory.proto";
import "cartographer/transform/proto/transform.proto";
message SparsePoseGraph {
message Constraint {
message SubmapId {
message SubmapId {
optional int32 trajectory_id = 1;
optional int32 submap_index = 2; // Submap index in the given trajectory.
}
}
message ScanId {
message ScanId {
optional int32 trajectory_id = 1;
optional int32 scan_index = 2; // Scan index in the given trajectory.
}
}
message SparsePoseGraph {
message Constraint {
// 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).

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

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

7
cartographer/mapping/submaps.h
View File

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

15
cartographer/mapping_2d/submaps.cc
View File

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

6
cartographer/mapping_2d/submaps.h
View File

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

6
cartographer/mapping_2d/submaps_test.cc
View File

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

17
cartographer/mapping_3d/submaps.cc
View File

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

6
cartographer/mapping_3d/submaps.h
View File

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

6
cartographer/mapping_3d/submaps_test.cc
View File

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