From 9042450aaf52a24c1c7559f0946b57a34605c815 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 5 Jul 2017 16:24:02 +0200 Subject: [PATCH] Fix initial pose of loaded submaps. (#391) PAIR=SirVer --- cartographer/io/proto_stream.cc | 2 +- cartographer/io/proto_stream.h | 2 +- cartographer/mapping/map_builder.cc | 2 +- cartographer/mapping_2d/sparse_pose_graph.cc | 18 +++++++++++++++--- 4 files changed, 18 insertions(+), 6 deletions(-) diff --git a/cartographer/io/proto_stream.cc b/cartographer/io/proto_stream.cc index c626311..e715e39 100644 --- a/cartographer/io/proto_stream.cc +++ b/cartographer/io/proto_stream.cc @@ -84,7 +84,7 @@ bool ProtoStreamReader::Read(string* decompressed_data) { return true; } -bool ProtoStreamReader::ok() const { return !in_.fail(); } +bool ProtoStreamReader::eof() const { return in_.eof(); } } // namespace io } // namespace cartographer diff --git a/cartographer/io/proto_stream.h b/cartographer/io/proto_stream.h index e8da567..ddc43b2 100644 --- a/cartographer/io/proto_stream.h +++ b/cartographer/io/proto_stream.h @@ -71,7 +71,7 @@ class ProtoStreamReader { proto->ParseFromString(decompressed_data); } - bool ok() const; + bool eof() const; private: bool Read(string* decompressed_data); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index ac66397..9644060 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -191,7 +191,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { proto.submap()); } } - CHECK(reader->ok()); + CHECK(reader->eof()); } int MapBuilder::num_trajectory_builders() const { diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 841eba7..c39baae 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -388,16 +388,28 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, std::shared_ptr submap_ptr = std::make_shared(submap.submap_2d()); + const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose); 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_) { + // Immediately show the submap at the optimized pose. + CHECK_GE(static_cast(submap_data_.num_trajectories()), + optimized_submap_transforms_.size()); + optimized_submap_transforms_.resize(submap_data_.num_trajectories()); + CHECK_GE(static_cast(submap_data_.num_trajectories()), + num_trimmed_submaps_at_last_optimization_.size()); + num_trimmed_submaps_at_last_optimization_.resize( + submap_data_.num_trajectories()); + CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(), + submap_id.submap_index); + optimized_submap_transforms_.at(trajectory_id) + .push_back(sparse_pose_graph::SubmapData{initial_pose_2d}); + AddWorkItem([this, submap_id, initial_pose_2d]() 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)); + optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose_2d); }); }