Serialize submaps. (#380)
parent
5396156968
commit
31f77a01d2
|
@ -135,7 +135,24 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
|
||||||
void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
|
void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
|
||||||
// We serialize the pose graph followed by all the data referenced in it.
|
// We serialize the pose graph followed by all the data referenced in it.
|
||||||
writer->WriteProto(sparse_pose_graph_->ToProto());
|
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 range data ("scan") for each trajectory node.
|
||||||
// TODO(whess): Serialize additional sensor data: IMU, odometry.
|
// TODO(whess): Serialize additional sensor data: IMU, odometry.
|
||||||
}
|
}
|
||||||
|
|
|
@ -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.
|
||||||
|
}
|
|
@ -19,8 +19,6 @@ package cartographer.mapping.proto;
|
||||||
import "cartographer/mapping/proto/trajectory.proto";
|
import "cartographer/mapping/proto/trajectory.proto";
|
||||||
import "cartographer/transform/proto/transform.proto";
|
import "cartographer/transform/proto/transform.proto";
|
||||||
|
|
||||||
message SparsePoseGraph {
|
|
||||||
message Constraint {
|
|
||||||
message SubmapId {
|
message SubmapId {
|
||||||
optional int32 trajectory_id = 1;
|
optional int32 trajectory_id = 1;
|
||||||
optional int32 submap_index = 2; // Submap index in the given trajectory.
|
optional int32 submap_index = 2; // Submap index in the given trajectory.
|
||||||
|
@ -31,6 +29,8 @@ message SparsePoseGraph {
|
||||||
optional int32 scan_index = 2; // Scan index in the given trajectory.
|
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
|
// Differentiates between intra-submap (where the scan was inserted into the
|
||||||
// submap) and inter-submap constraints (where the scan was not inserted
|
// submap) and inter-submap constraints (where the scan was not inserted
|
||||||
// into the submap).
|
// into the submap).
|
||||||
|
|
|
@ -16,15 +16,23 @@ syntax = "proto2";
|
||||||
|
|
||||||
package cartographer.mapping.proto;
|
package cartographer.mapping.proto;
|
||||||
|
|
||||||
import "cartographer/transform/proto/transform.proto";
|
|
||||||
import "cartographer/mapping_2d/proto/probability_grid.proto";
|
import "cartographer/mapping_2d/proto/probability_grid.proto";
|
||||||
import "cartographer/mapping_3d/proto/hybrid_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 transform.proto.Rigid3d local_pose = 1;
|
||||||
optional int32 num_range_data = 2;
|
optional int32 num_range_data = 2;
|
||||||
optional bool finished = 3;
|
optional bool finished = 3;
|
||||||
optional mapping_2d.proto.ProbabilityGrid probability_grid = 4;
|
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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -25,6 +25,7 @@
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/mapping/id.h"
|
#include "cartographer/mapping/id.h"
|
||||||
#include "cartographer/mapping/probability_values.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/proto/submap_visualization.pb.h"
|
||||||
#include "cartographer/mapping/trajectory_node.h"
|
#include "cartographer/mapping/trajectory_node.h"
|
||||||
#include "cartographer/mapping_2d/probability_grid.h"
|
#include "cartographer/mapping_2d/probability_grid.h"
|
||||||
|
@ -61,6 +62,8 @@ class Submap {
|
||||||
Submap(const transform::Rigid3d& local_pose) : local_pose_(local_pose) {}
|
Submap(const transform::Rigid3d& local_pose) : local_pose_(local_pose) {}
|
||||||
virtual ~Submap() {}
|
virtual ~Submap() {}
|
||||||
|
|
||||||
|
virtual void ToProto(proto::Submap* proto) const = 0;
|
||||||
|
|
||||||
// Local SLAM pose of this submap.
|
// Local SLAM pose of this submap.
|
||||||
transform::Rigid3d local_pose() const { return local_pose_; }
|
transform::Rigid3d local_pose() const { return local_pose_; }
|
||||||
|
|
||||||
|
@ -73,7 +76,9 @@ class Submap {
|
||||||
proto::SubmapQuery::Response* response) const = 0;
|
proto::SubmapQuery::Response* response) const = 0;
|
||||||
|
|
||||||
protected:
|
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:
|
private:
|
||||||
const transform::Rigid3d local_pose_;
|
const transform::Rigid3d local_pose_;
|
||||||
|
|
|
@ -69,20 +69,19 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
|
||||||
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
|
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
|
||||||
probability_grid_(limits) {}
|
probability_grid_(limits) {}
|
||||||
|
|
||||||
Submap::Submap(const mapping::proto::Submap& proto)
|
Submap::Submap(const mapping::proto::Submap2D& proto)
|
||||||
: mapping::Submap(transform::ToRigid3(proto.local_pose())),
|
: mapping::Submap(transform::ToRigid3(proto.local_pose())),
|
||||||
probability_grid_(ProbabilityGrid(proto.probability_grid())) {
|
probability_grid_(ProbabilityGrid(proto.probability_grid())) {
|
||||||
SetNumRangeData(proto.num_range_data());
|
SetNumRangeData(proto.num_range_data());
|
||||||
finished_ = proto.finished();
|
finished_ = proto.finished();
|
||||||
}
|
}
|
||||||
|
|
||||||
mapping::proto::Submap Submap::ToProto() const {
|
void Submap::ToProto(mapping::proto::Submap* const proto) const {
|
||||||
mapping::proto::Submap proto;
|
auto* const submap_2d = proto->mutable_submap_2d();
|
||||||
*proto.mutable_local_pose() = transform::ToProto(local_pose());
|
*submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
|
||||||
proto.set_num_range_data(num_range_data());
|
submap_2d->set_num_range_data(num_range_data());
|
||||||
proto.set_finished(finished_);
|
submap_2d->set_finished(finished_);
|
||||||
*proto.mutable_probability_grid() = probability_grid_.ToProto();
|
*submap_2d->mutable_probability_grid() = probability_grid_.ToProto();
|
||||||
return proto;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Submap::ToResponseProto(
|
void Submap::ToResponseProto(
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#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/proto/submap_visualization.pb.h"
|
||||||
#include "cartographer/mapping/submaps.h"
|
#include "cartographer/mapping/submaps.h"
|
||||||
#include "cartographer/mapping/trajectory_node.h"
|
#include "cartographer/mapping/trajectory_node.h"
|
||||||
|
@ -45,9 +45,9 @@ proto::SubmapsOptions CreateSubmapsOptions(
|
||||||
class Submap : public mapping::Submap {
|
class Submap : public mapping::Submap {
|
||||||
public:
|
public:
|
||||||
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
|
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_; }
|
const ProbabilityGrid& probability_grid() const { return probability_grid_; }
|
||||||
bool finished() const { return finished_; }
|
bool finished() const { return finished_; }
|
||||||
|
|
|
@ -71,7 +71,11 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
|
||||||
TEST(SubmapsTest, ToFromProto) {
|
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));
|
||||||
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(
|
EXPECT_TRUE(expected.local_pose().translation().isApprox(
|
||||||
actual.local_pose().translation(), 1e-6));
|
actual.local_pose().translation(), 1e-6));
|
||||||
EXPECT_TRUE(expected.local_pose().rotation().isApprox(
|
EXPECT_TRUE(expected.local_pose().rotation().isApprox(
|
||||||
|
|
|
@ -326,7 +326,7 @@ Submap::Submap(const float high_resolution, const float low_resolution,
|
||||||
high_resolution_hybrid_grid_(high_resolution),
|
high_resolution_hybrid_grid_(high_resolution),
|
||||||
low_resolution_hybrid_grid_(low_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())),
|
: mapping::Submap(transform::ToRigid3(proto.local_pose())),
|
||||||
high_resolution_hybrid_grid_(proto.high_resolution_hybrid_grid()),
|
high_resolution_hybrid_grid_(proto.high_resolution_hybrid_grid()),
|
||||||
low_resolution_hybrid_grid_(proto.low_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();
|
finished_ = proto.finished();
|
||||||
}
|
}
|
||||||
|
|
||||||
mapping::proto::Submap Submap::ToProto() const {
|
void Submap::ToProto(mapping::proto::Submap* const proto) const {
|
||||||
mapping::proto::Submap proto;
|
auto* const submap_3d = proto->mutable_submap_3d();
|
||||||
*proto.mutable_local_pose() = transform::ToProto(local_pose());
|
*submap_3d->mutable_local_pose() = transform::ToProto(local_pose());
|
||||||
proto.set_num_range_data(num_range_data());
|
submap_3d->set_num_range_data(num_range_data());
|
||||||
proto.set_finished(finished_);
|
submap_3d->set_finished(finished_);
|
||||||
*proto.mutable_high_resolution_hybrid_grid() =
|
*submap_3d->mutable_high_resolution_hybrid_grid() =
|
||||||
mapping_3d::ToProto(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());
|
mapping_3d::ToProto(low_resolution_hybrid_grid());
|
||||||
return proto;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Submap::ToResponseProto(
|
void Submap::ToResponseProto(
|
||||||
|
|
|
@ -24,7 +24,7 @@
|
||||||
#include "Eigen/Geometry"
|
#include "Eigen/Geometry"
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/mapping/id.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/proto/submap_visualization.pb.h"
|
||||||
#include "cartographer/mapping/submaps.h"
|
#include "cartographer/mapping/submaps.h"
|
||||||
#include "cartographer/mapping_2d/probability_grid.h"
|
#include "cartographer/mapping_2d/probability_grid.h"
|
||||||
|
@ -52,9 +52,9 @@ class Submap : public mapping::Submap {
|
||||||
public:
|
public:
|
||||||
Submap(float high_resolution, float low_resolution,
|
Submap(float high_resolution, float low_resolution,
|
||||||
const transform::Rigid3d& local_pose);
|
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 {
|
const HybridGrid& high_resolution_hybrid_grid() const {
|
||||||
return high_resolution_hybrid_grid_;
|
return high_resolution_hybrid_grid_;
|
||||||
|
|
|
@ -27,7 +27,11 @@ TEST(SubmapsTest, ToFromProto) {
|
||||||
const Submap expected(0.05, 0.25,
|
const Submap expected(0.05, 0.25,
|
||||||
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.)));
|
||||||
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(
|
EXPECT_TRUE(expected.local_pose().translation().isApprox(
|
||||||
actual.local_pose().translation(), 1e-6));
|
actual.local_pose().translation(), 1e-6));
|
||||||
EXPECT_TRUE(expected.local_pose().rotation().isApprox(
|
EXPECT_TRUE(expected.local_pose().rotation().isApprox(
|
||||||
|
|
Loading…
Reference in New Issue