From 9c81a01608303dba40903d3f33d7e2d7c6cbfdef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Mon, 2 Oct 2017 17:01:14 +0200 Subject: [PATCH] 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. --- cartographer/mapping_3d/sparse_pose_graph.cc | 60 ++++++++++++++++++- .../sparse_pose_graph/constraint_builder.cc | 6 ++ .../sparse_pose_graph/constraint_builder.h | 3 + .../sparse_pose_graph/optimization_problem.cc | 19 ++++++ .../sparse_pose_graph/optimization_problem.h | 2 + 5 files changed, 88 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 286b992..0675878 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -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 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 nodes_to_remove; + { + std::vector 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 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 diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index a589e06..b479a6e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -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 diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index d3161dc..80ae20d 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -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; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 58b627b..b2671ad 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -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(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); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 08eb6ef..3657955 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -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);