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

Fix initial pose of loaded submaps. (#391) 

PAIR=SirVer
master
Wolfgang Hess 2017-07-05 16:24:02 +02:00 committed by GitHub
parent 3819dd3806
commit 9042450aaf
4 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/io/proto_stream.cc
View File

@ -84,7 +84,7 @@ bool ProtoStreamReader::Read(string* decompressed_data) {
return true; return true;
} }
bool ProtoStreamReader::ok() const { return !in_.fail(); } bool ProtoStreamReader::eof() const { return in_.eof(); }
} // namespace io } // namespace io
} // namespace cartographer } // namespace cartographer

2
cartographer/io/proto_stream.h
View File

@ -71,7 +71,7 @@ class ProtoStreamReader {
proto->ParseFromString(decompressed_data); proto->ParseFromString(decompressed_data);
} }
bool ok() const; bool eof() const;
private: private:
bool Read(string* decompressed_data); bool Read(string* decompressed_data);

2
cartographer/mapping/map_builder.cc
View File

@ -191,7 +191,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
proto.submap()); proto.submap());
} }
} }
CHECK(reader->ok()); CHECK(reader->eof());
} }
int MapBuilder::num_trajectory_builders() const { int MapBuilder::num_trajectory_builders() const {

18
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -388,16 +388,28 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
std::shared_ptr<const Submap> submap_ptr = std::shared_ptr<const Submap> submap_ptr =
std::make_shared<const Submap>(submap.submap_2d()); std::make_shared<const Submap>(submap.submap_2d());
const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const mapping::SubmapId submap_id = const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData()); submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr; 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<size_t>(submap_data_.num_trajectories()),
optimized_submap_transforms_.size());
optimized_submap_transforms_.resize(submap_data_.num_trajectories());
CHECK_GE(static_cast<size_t>(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); CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.AddSubmap(submap_id.trajectory_id, optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose_2d);
transform::Project2D(initial_pose));
}); });
} }