Add trajectory trimming support to 3d (#559)

* Adds DeleteScanMatcher() to 3D constraint builder
* Add possibility to remove submaps and scans from the 3D optimization
  problem.
* Implements TrimmingHandle in SPA.
master
Christoph Schütte 2017-10-02 17:01:14 +02:00 committed by GitHub
parent 3c521eb573
commit 9c81a01608
5 changed files with 88 additions and 2 deletions

View File

@ -648,12 +648,68 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
int SparsePoseGraph::TrimmingHandle::num_submaps(
const int trajectory_id) const {
LOG(FATAL) << "Not yet implemented for 3D.";
return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
}
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) {
LOG(FATAL) << "Not yet implemented for 3D.";
// TODO(hrapp): We have to make sure that the trajectory has been finished
// if we want to delete the last submaps.
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
// 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);
}
// Mark the submap with 'submap_id' as trimmed and remove its data.
auto& submap_data = parent_->submap_data_.at(submap_id);
CHECK(submap_data.state == SubmapState::kFinished);
submap_data.state = SubmapState::kTrimmed;
CHECK(submap_data.submap != nullptr);
submap_data.submap.reset();
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id);
// Mark the 'nodes_to_remove' as trimmed and remove their data.
for (const mapping::NodeId& node_id : nodes_to_remove) {
CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed());
parent_->trajectory_nodes_.at(node_id).constant_data.reset();
parent_->optimization_problem_.TrimTrajectoryNode(node_id);
}
}
} // namespace mapping_3d

View File

@ -294,6 +294,12 @@ int ConstraintBuilder::GetNumFinishedScans() {
return pending_computations_.begin()->first;
}
void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
CHECK(pending_computations_.empty());
submap_scan_matchers_.erase(submap_id);
}
} // namespace sparse_pose_graph
} // namespace mapping_3d
} // namespace cartographer

View File

@ -105,6 +105,9 @@ class ConstraintBuilder {
// Returns the number of consecutive finished scans.
int GetNumFinishedScans();
// Delete data related to 'submap_id'.
void DeleteScanMatcher(const mapping::SubmapId& submap_id);
private:
struct SubmapScanMatcher {
const HybridGrid* high_resolution_hybrid_grid;

View File

@ -93,6 +93,20 @@ void OptimizationProblem::AddTrajectoryNode(
++trajectory_data.next_node_index;
}
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
auto& node_data = node_data_.at(node_id.trajectory_id);
CHECK(node_data.erase(node_id.node_index));
if (!node_data.empty() &&
node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
const common::Time node_time = node_data.begin()->second.time;
auto& imu_data = imu_data_.at(node_id.trajectory_id);
while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
imu_data.pop_front();
}
}
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid3d& submap_pose) {
CHECK_GE(trajectory_id, 0);
@ -106,6 +120,11 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
++trajectory_data.next_submap_index;
}
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
auto& submap_data = submap_data_.at(submap_id.trajectory_id);
CHECK(submap_data.erase(submap_id.submap_index));
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations);

View File

@ -73,7 +73,9 @@ class OptimizationProblem {
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);