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

Remove trimmed constraints. (#317) 

Still missing marking submaps and nodes as trimmed.

PAIR=SirVer
master
Wolfgang Hess 2017-06-07 14:23:58 +02:00 committed by GitHub
parent a66147f131
commit b3b9735a4b
2 changed files with 51 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/id.h
View File

@ -52,6 +52,8 @@ struct SubmapId {
std::forward_as_tuple(other.trajectory_id, other.submap_index);
}
bool operator!=(const SubmapId& other) const { return !operator==(other); }
bool operator<(const SubmapId& other) const {
return std::forward_as_tuple(trajectory_id, submap_index) <
std::forward_as_tuple(other.trajectory_id, other.submap_index);

50
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -24,6 +24,7 @@
#include <iostream>
#include <limits>
#include <memory>
#include <set>
#include <sstream>
#include <string>
@ -492,7 +493,54 @@ int SparsePoseGraph::TrimmingHandle::num_submaps(
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) {
LOG(FATAL) << "Not yet implemented.";
// Compile all nodes that are still INTRA_SUBMAP constrained once the submap
// with 'submap_id' is gone.
std::set<mapping::NodeId> nodes_to_retain;
for (const Constraint& constraint : parent_->constraints_) {
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
constraint.submap_id != submap_id) {
nodes_to_retain.insert(constraint.node_id);
}
}
// Remove all 'constraints_' related to 'submap_id'.
std::set<mapping::NodeId> nodes_to_remove;
{
std::vector<Constraint> constraints;
for (const Constraint& constraint : parent_->constraints_) {
if (constraint.submap_id == submap_id) {
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
nodes_to_retain.count(constraint.node_id) == 0) {
// This node will no longer be INTRA_SUBMAP contrained and has to be
// removed.
nodes_to_remove.insert(constraint.node_id);
}
} else {
constraints.push_back(constraint);
}
}
parent_->constraints_ = std::move(constraints);
}
// Remove all 'constraints_' related to 'nodes_to_remove'.
{
std::vector<Constraint> constraints;
for (const Constraint& constraint : parent_->constraints_) {
if (nodes_to_remove.count(constraint.node_id) == 0) {
constraints.push_back(constraint);
}
}
parent_->constraints_ = std::move(constraints);
}
// TODO(whess): Mark the submap with 'submap_id' as pruned and remove its
// data. Also make sure we no longer try to scan match against it.
// TODO(whess): Mark the 'nodes_to_remove' as pruned and remove their data.
// Also make sure we no longer try to scan match against it.
// TODO(whess): The optimization problem should no longer include the submap
// and the removed nodes.
// TODO(whess): If the first submap is gone, we want to tie the first not yet
// trimmed submap to be set fixed to its current pose.
}
} // namespace mapping_2d