From 7434e963697c2876add9658e1ff1af6399589215 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 4 Jul 2017 17:20:13 +0200 Subject: [PATCH] First rough implementation of loading a map. (#382) This implements loading of serialized maps in 2D: We insert all submaps at the serialized optimized pose into the pose graph and remove their poses from the optimization. Related to #197 and #315. --- cartographer/io/proto_stream.cc | 2 + cartographer/io/proto_stream.h | 2 + cartographer/mapping/map_builder.cc | 37 +++++++++++++++++++ cartographer/mapping/map_builder.h | 3 ++ cartographer/mapping/sparse_pose_graph.h | 10 +++++ cartographer/mapping_2d/sparse_pose_graph.cc | 32 +++++++++++++++- cartographer/mapping_2d/sparse_pose_graph.h | 7 ++++ .../sparse_pose_graph/optimization_problem.cc | 9 +++-- .../sparse_pose_graph/optimization_problem.h | 3 +- cartographer/mapping_3d/sparse_pose_graph.h | 8 ++++ 10 files changed, 108 insertions(+), 5 deletions(-) diff --git a/cartographer/io/proto_stream.cc b/cartographer/io/proto_stream.cc index 7f47553..c626311 100644 --- a/cartographer/io/proto_stream.cc +++ b/cartographer/io/proto_stream.cc @@ -84,5 +84,7 @@ bool ProtoStreamReader::Read(string* decompressed_data) { return true; } +bool ProtoStreamReader::ok() const { return !in_.fail(); } + } // namespace io } // namespace cartographer diff --git a/cartographer/io/proto_stream.h b/cartographer/io/proto_stream.h index 5525041..e8da567 100644 --- a/cartographer/io/proto_stream.h +++ b/cartographer/io/proto_stream.h @@ -71,6 +71,8 @@ class ProtoStreamReader { proto->ParseFromString(decompressed_data); } + bool ok() const; + private: bool Read(string* decompressed_data); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 5c550db..ac66397 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -157,6 +157,43 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { // TODO(whess): Serialize additional sensor data: IMU, odometry. } +void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { + proto::SparsePoseGraph pose_graph; + CHECK(reader->ReadProto(&pose_graph)); + + // TODO(whess): Not all trajectories should be builders, i.e. support should + // be added for trajectories without latest pose, options, etc. Appease the + // trajectory builder for now. + proto::TrajectoryBuilderOptions unused_options; + unused_options.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->set_resolution(0.05); + unused_options.mutable_trajectory_builder_2d_options() + ->set_num_odometry_states(1); + + const std::unordered_set unused_sensor_ids; + const int map_trajectory_id = + AddTrajectoryBuilder(unused_sensor_ids, unused_options); + FinishTrajectory(map_trajectory_id); + sparse_pose_graph_->FreezeTrajectory(map_trajectory_id); + + for (;;) { + proto::SerializedData proto; + if (!reader->ReadProto(&proto)) { + break; + } + if (proto.has_submap()) { + const transform::Rigid3d submap_pose = transform::ToRigid3( + pose_graph.trajectory(proto.submap().submap_id().trajectory_id()) + .submap(proto.submap().submap_id().submap_index()) + .pose()); + sparse_pose_graph_->AddSubmapFromProto(map_trajectory_id, submap_pose, + proto.submap()); + } + } + CHECK(reader->ok()); +} + int MapBuilder::num_trajectory_builders() const { return trajectory_builders_.size(); } diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index ac94d2f..d6efbea 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -81,6 +81,9 @@ class MapBuilder { // Serializes the current state to a proto stream. void SerializeState(io::ProtoStreamWriter* writer); + // Loads submaps from a proto stream into a new frozen trajectory. + void LoadMap(io::ProtoStreamReader* reader); + int num_trajectory_builders() const; mapping::SparsePoseGraph* sparse_pose_graph(); diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 86c3ddb..58e6e35 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -26,6 +26,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/pose_graph_trimmer.h" +#include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" #include "cartographer/mapping/submaps.h" @@ -73,6 +74,15 @@ class SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; + // Freezes a trajectory. Poses in this trajectory will not be optimized. + virtual void FreezeTrajectory(int trajectory_id) = 0; + + // Adds a 'submap' from a proto with the given 'initial_pose' to the frozen + // trajectory with 'trajectory_id'. + virtual void AddSubmapFromProto(int trajectory_id, + const transform::Rigid3d& initial_pose, + const proto::Submap& submap) = 0; + // Adds a 'trimmer'. It will be used after all data added before it has been // included in the pose graph. virtual void AddTrimmer(std::unique_ptr trimmer) = 0; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c32a56f..841eba7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -371,6 +371,36 @@ void SparsePoseGraph::WaitForAllComputations() { locker.Await([¬ification]() { return notification; }); } +void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { + common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { + CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0); + frozen_trajectories_.insert(trajectory_id); + }); +} + +void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, + const transform::Rigid3d& initial_pose, + const mapping::proto::Submap& submap) { + if (!submap.has_submap_2d()) { + return; + } + + std::shared_ptr submap_ptr = + std::make_shared(submap.submap_2d()); + + common::MutexLocker locker(&mutex_); + const mapping::SubmapId submap_id = + submap_data_.Append(trajectory_id, SubmapData()); + submap_data_.at(submap_id).submap = submap_ptr; + AddWorkItem([this, submap_id, initial_pose]() REQUIRES(mutex_) { + CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); + submap_data_.at(submap_id).state = SubmapState::kFinished; + optimization_problem_.AddSubmap(submap_id.trajectory_id, + transform::Project2D(initial_pose)); + }); +} + void SparsePoseGraph::AddTrimmer( std::unique_ptr trimmer) { common::MutexLocker locker(&mutex_); @@ -395,7 +425,7 @@ void SparsePoseGraph::RunOptimization() { if (optimization_problem_.submap_data().empty()) { return; } - optimization_problem_.Solve(constraints_); + optimization_problem_.Solve(constraints_, frozen_trajectories_); common::MutexLocker locker(&mutex_); std::vector num_trimmed_submaps; diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 2c61b30..732e99e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -80,6 +80,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); + void FreezeTrajectory(int trajectory_id) override; + void AddSubmapFromProto(int trajectory_id, + const transform::Rigid3d& initial_pose, + const mapping::proto::Submap& submap) override; void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; @@ -208,6 +212,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> trimmers_ GUARDED_BY(mutex_); + // Set of all frozen trajectories not being optimized. + std::set frozen_trajectories_ GUARDED_BY(mutex_); + // Allows querying and manipulating the pose graph by the 'trimmers_'. The // 'mutex_' of the pose graph is held while this class is used. class TrimmingHandle : public mapping::Trimmable { diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 6c9ee32..caf15cf 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -123,7 +123,8 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { max_num_iterations); } -void OptimizationProblem::Solve(const std::vector& constraints) { +void OptimizationProblem::Solve(const std::vector& constraints, + const std::set& frozen_trajectories) { if (node_data_.empty()) { // Nothing to optimize. return; @@ -140,15 +141,17 @@ void OptimizationProblem::Solve(const std::vector& constraints) { bool first_submap = true; for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { + const bool frozen = frozen_trajectories.count(trajectory_id); // Reserve guarantees that data does not move, so the pointers for Ceres // stay valid. C_submaps[trajectory_id].reserve(submap_data_[trajectory_id].size()); for (const SubmapData& submap_data : submap_data_[trajectory_id]) { C_submaps[trajectory_id].push_back(FromPose(submap_data.pose)); problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); - if (first_submap) { + if (first_submap || frozen) { first_submap = false; - // Fix the pose of the first submap. + // Fix the pose of the first submap or all submaps of a frozen + // trajectory. problem.SetParameterBlockConstant( C_submaps[trajectory_id].back().data()); } diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 5683c5f..3704774 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -70,7 +70,8 @@ class OptimizationProblem { void SetMaxNumIterations(int32 max_num_iterations); // Computes the optimized poses. - void Solve(const std::vector& constraints); + void Solve(const std::vector& constraints, + const std::set& frozen_trajectories); const std::vector>& node_data() const; const std::vector>& submap_data() const; diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index b515efb..6e459b6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -77,6 +77,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); + void FreezeTrajectory(const int trajectory_id) override { + LOG(FATAL) << "Not yet implemented for 3D."; + } + void AddSubmapFromProto(const int trajectory_id, + const transform::Rigid3d& initial_pose, + const mapping::proto::Submap& submap) override { + LOG(FATAL) << "Not yet implemented for 3D"; + } void AddTrimmer(std::unique_ptr trimmer) override { LOG(FATAL) << "Not yet implemented for 3D."; }