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

34
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -299,16 +299,13 @@ common::Time SparsePoseGraph::GetLatestScanTime(
}
void SparsePoseGraph::UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result) {
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const common::Time time =
GetLatestScanTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);
}
const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const common::Time time =
GetLatestScanTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);
}
void SparsePoseGraph::HandleWorkQueue() {
@ -321,7 +318,9 @@ void SparsePoseGraph::HandleWorkQueue() {
RunOptimization();
common::MutexLocker locker(&mutex_);
UpdateTrajectoryConnectivity(result);
for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
@ -460,10 +459,15 @@ void SparsePoseGraph::AddSerializedConstraints(
CHECK(submap_data_.Contains(constraint.submap_id));
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
CHECK(submap_data_.at(constraint.submap_id).submap != nullptr);
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
CHECK(submap_data_.at(constraint.submap_id)
.node_ids.emplace(constraint.node_id)
.second);
switch (constraint.tag) {
case Constraint::Tag::INTRA_SUBMAP:
CHECK(submap_data_.at(constraint.submap_id)
.node_ids.emplace(constraint.node_id)
.second);
break;
case Constraint::Tag::INTER_SUBMAP:
UpdateTrajectoryConnectivity(constraint);
break;
}
const Constraint::Pose pose = {
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
REQUIRES(mutex_);
// Updates the trajectory connectivity structure with the new constraints.
void UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result)
// Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity(const Constraint& constraint)
REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_;

34
cartographer/mapping_3d/sparse_pose_graph.cc
View File

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

5
cartographer/mapping_3d/sparse_pose_graph.h
View File

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