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

Deserialize trajectory connectivity (#552)

master
Juraj Oršulić 2017-11-14 13:26:53 +01:00 committed by Wally B. Feed
parent 3ec583a327
commit 60c72cb445
4 changed files with 42 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -299,16 +299,13 @@ common::Time SparsePoseGraph::GetLatestScanTime(
} }
void SparsePoseGraph::UpdateTrajectoryConnectivity( void SparsePoseGraph::UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result) { const Constraint& constraint) {
for (const Constraint& constraint : result) { CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const common::Time time = const common::Time time =
GetLatestScanTime(constraint.node_id, constraint.submap_id); GetLatestScanTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id, constraint.submap_id.trajectory_id,
time); time);
}
} }
void SparsePoseGraph::HandleWorkQueue() { void SparsePoseGraph::HandleWorkQueue() {
@ -321,7 +318,9 @@ void SparsePoseGraph::HandleWorkQueue() {
RunOptimization(); RunOptimization();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
UpdateTrajectoryConnectivity(result); for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
TrimmingHandle trimming_handle(this); TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) { for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle); trimmer->Trim(&trimming_handle);
@ -460,10 +459,15 @@ void SparsePoseGraph::AddSerializedConstraints(
CHECK(submap_data_.Contains(constraint.submap_id)); CHECK(submap_data_.Contains(constraint.submap_id));
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr); CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
CHECK(submap_data_.at(constraint.submap_id).submap != nullptr); CHECK(submap_data_.at(constraint.submap_id).submap != nullptr);
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { switch (constraint.tag) {
case Constraint::Tag::INTRA_SUBMAP:
CHECK(submap_data_.at(constraint.submap_id) CHECK(submap_data_.at(constraint.submap_id)
.node_ids.emplace(constraint.node_id) .node_ids.emplace(constraint.node_id)
.second); .second);
break;
case Constraint::Tag::INTER_SUBMAP:
UpdateTrajectoryConnectivity(constraint);
break;
} }
const Constraint::Pose pose = { const Constraint::Pose pose = {
constraint.pose.zbar_ij * constraint.pose.zbar_ij *

5
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -183,9 +183,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::SubmapId& submap_id) const const mapping::SubmapId& submap_id) const
REQUIRES(mutex_); REQUIRES(mutex_);
// Updates the trajectory connectivity structure with the new constraints. // Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity( void UpdateTrajectoryConnectivity(const Constraint& constraint)
const sparse_pose_graph::ConstraintBuilder::Result& result)
REQUIRES(mutex_); REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_; const mapping::proto::SparsePoseGraphOptions options_;

18
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -313,16 +313,13 @@ common::Time SparsePoseGraph::GetLatestScanTime(
} }
void SparsePoseGraph::UpdateTrajectoryConnectivity( void SparsePoseGraph::UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result) { const Constraint& constraint) {
for (const Constraint& constraint : result) { CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const common::Time time = const common::Time time =
GetLatestScanTime(constraint.node_id, constraint.submap_id); GetLatestScanTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id, constraint.submap_id.trajectory_id,
time); time);
}
} }
void SparsePoseGraph::HandleWorkQueue() { void SparsePoseGraph::HandleWorkQueue() {
@ -335,7 +332,9 @@ void SparsePoseGraph::HandleWorkQueue() {
RunOptimization(); RunOptimization();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
UpdateTrajectoryConnectivity(result); for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
TrimmingHandle trimming_handle(this); TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) { for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle); trimmer->Trim(&trimming_handle);
@ -466,10 +465,15 @@ void SparsePoseGraph::AddSerializedConstraints(
CHECK(submap_data_.Contains(constraint.submap_id)); CHECK(submap_data_.Contains(constraint.submap_id));
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr); CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
CHECK(submap_data_.at(constraint.submap_id).submap != nullptr); CHECK(submap_data_.at(constraint.submap_id).submap != nullptr);
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { switch (constraint.tag) {
case Constraint::Tag::INTRA_SUBMAP:
CHECK(submap_data_.at(constraint.submap_id) CHECK(submap_data_.at(constraint.submap_id)
.node_ids.emplace(constraint.node_id) .node_ids.emplace(constraint.node_id)
.second); .second);
break;
case Constraint::Tag::INTER_SUBMAP:
UpdateTrajectoryConnectivity(constraint);
break;
} }
constraints_.push_back(constraint); constraints_.push_back(constraint);
} }

5
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -187,9 +187,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// poses. // poses.
void LogResidualHistograms() REQUIRES(mutex_); void LogResidualHistograms() REQUIRES(mutex_);
// Updates the trajectory connectivity structure with the new constraints. // Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity( void UpdateTrajectoryConnectivity(const Constraint& constraint)
const sparse_pose_graph::ConstraintBuilder::Result& result)
REQUIRES(mutex_); REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_; const mapping::proto::SparsePoseGraphOptions options_;