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

Update trajectory connection in WhenDone callback. (#506) 

This modifies the ConstraintBuilder interface so that Maybe*Constraint
is not responsible anymore for updating the TrajectoryConnectivity
member of SparsePoseGraph and moves this responsibility into the
WhenDone callback.
master
Christoph Schütte 2017-09-07 14:51:19 +02:00 committed by Wolfgang Hess
parent 333516d955
commit a5dafcfde3
8 changed files with 30 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -178,8 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(),
&trajectory_connectivity_);
trajectory_nodes_.at(node_id).constant_data.get());
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
@ -313,6 +312,15 @@ void SparsePoseGraph::HandleWorkQueue() {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
// Update the trajectory connectivity structure with the new
// constraints.
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id);
}
RunOptimization();
common::MutexLocker locker(&mutex_);

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -172,7 +172,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
// How our various trajectories are related.
mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
mapping::TrajectoryConnectivity trajectory_connectivity_;
// We globally localize a fraction of the scans from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

9
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -77,7 +77,6 @@ void ConstraintBuilder::MaybeAddConstraint(
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
constant_data, initial_relative_pose, constraint);
FinishComputation(current_computation);
});
@ -87,8 +86,7 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
const mapping::TrajectoryNode::Data* const constant_data) {
common::MutexLocker locker(&mutex_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
@ -98,7 +96,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
trajectory_connectivity, constant_data,
constant_data,
transform::Rigid2d::Identity(), constraint);
FinishComputation(current_computation);
});
@ -162,7 +160,6 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
@ -190,8 +187,6 @@ void ConstraintBuilder::ComputeConstraint(
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_connectivity->Connect(node_id.trajectory_id,
submap_id.trajectory_id);
} else {
return;
}

12
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -32,7 +32,6 @@
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h"
@ -87,15 +86,12 @@ class ConstraintBuilder {
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
// This performs full-submap matching.
//
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
mapping::TrajectoryConnectivity* trajectory_connectivity);
const mapping::TrajectoryNode::Data* const constant_data);
// Must be called after all computations related to one node have been added.
void NotifyEndOfScan();
@ -134,14 +130,10 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change
// anymore. If 'match_full_submap' is true, and global localization succeeds,
// will connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
// anymore. As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);

11
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -210,7 +210,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose.rotation(), &trajectory_connectivity_);
initial_relative_pose.rotation());
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
@ -328,6 +328,15 @@ void SparsePoseGraph::HandleWorkQueue() {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
// Update the trajectory connectivity structure with the new
// constraints.
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id);
}
RunOptimization();
common::MutexLocker locker(&mutex_);

2
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -177,7 +177,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
// How our various trajectories are related.
mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
mapping::TrajectoryConnectivity trajectory_connectivity_;
// We globally localize a fraction of the scans from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

9
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -72,7 +72,6 @@ void ConstraintBuilder::MaybeAddConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
constant_data, initial_pose, constraint);
FinishComputation(current_computation);
});
@ -84,8 +83,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment,
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
const Eigen::Quaterniond& gravity_alignment) {
common::MutexLocker locker(&mutex_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
@ -94,7 +92,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
trajectory_connectivity, constant_data,
constant_data,
transform::Rigid3d::Rotation(gravity_alignment),
constraint);
FinishComputation(current_computation);
@ -167,7 +165,6 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& initial_pose,
std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
@ -194,8 +191,6 @@ void ConstraintBuilder::ComputeConstraint(
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_connectivity->Connect(node_id.trajectory_id,
submap_id.trajectory_id);
} else {
return;
}

11
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -31,7 +31,6 @@
#include "cartographer/common/math.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
@ -85,8 +84,7 @@ class ConstraintBuilder {
// This performs full-submap matching.
//
// The 'gravity_alignment' is the rotation to apply to the point cloud data
// to make it approximately gravity aligned. The 'trajectory_connectivity' is
// updated if the full-submap match succeeds.
// to make it approximately gravity aligned.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
@ -95,8 +93,7 @@ class ConstraintBuilder {
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment,
mapping::TrajectoryConnectivity* trajectory_connectivity);
const Eigen::Quaterniond& gravity_alignment);
// Must be called after all computations related to one node have been added.
void NotifyEndOfScan();
@ -136,14 +133,10 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional
// constraint.
// If 'match_full_submap' is true, and global localization succeeds, will
// connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& initial_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);