From c90b887bfbb62aba873e60bddb2ccbcac6c826c3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 6 Jun 2017 17:07:30 +0200 Subject: [PATCH] Adds API to add trimmers. (#314) This is related to #283. First to be implemented for pure localization and 2D SLAM. PAIR=SirVer --- cartographer/mapping/pose_graph_trimmer.h | 53 ++++++++++++++++++++ cartographer/mapping/sparse_pose_graph.h | 8 ++- cartographer/mapping_2d/sparse_pose_graph.cc | 28 +++++++++++ cartographer/mapping_2d/sparse_pose_graph.h | 21 ++++++++ cartographer/mapping_3d/sparse_pose_graph.h | 3 ++ 5 files changed, 112 insertions(+), 1 deletion(-) create mode 100644 cartographer/mapping/pose_graph_trimmer.h diff --git a/cartographer/mapping/pose_graph_trimmer.h b/cartographer/mapping/pose_graph_trimmer.h new file mode 100644 index 0000000..f3f1d98 --- /dev/null +++ b/cartographer/mapping/pose_graph_trimmer.h @@ -0,0 +1,53 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_ + +#include "cartographer/mapping/id.h" + +namespace cartographer { +namespace mapping { + +// Implemented by the pose graph to provide thread-safe access to functions for +// trimming the graph. +class TrimmingInterface { + public: + virtual ~TrimmingInterface() {} + + // TODO(whess): This is all the functionality necessary for pure localization. + // To be expanded as needed for lifelong mapping. + virtual int num_submaps(int trajectory_id) const = 0; + + // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They + // will no longer take part in scan matching, loop closure, visualization. + // Submaps and nodes are only marked, the numbering remains unchanged. + virtual void MarkSubmapAsTrimmed(const SubmapId& submap_id) = 0; +}; + +// An interface to implement algorithms that choose how to trim the pose graph. +class PoseGraphTrimmer { + public: + virtual ~PoseGraphTrimmer() {} + + // Called once after each pose graph optimization. + virtual void Trim(TrimmingInterface* trimming) = 0; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_ diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index a1a43ed..229097c 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ +#include #include #include #include @@ -24,6 +25,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" #include "cartographer/mapping/trajectory_node.h" @@ -65,10 +67,14 @@ class SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; + // Adds a 'trimmer'. It will be used after all data added before it has been + // included in the pose graph. + virtual void AddTrimmer(std::unique_ptr trimmer) = 0; + // Computes optimized poses. virtual void RunFinalOptimization() = 0; - // Get the current trajectory clusters. + // Gets the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() = 0; // Returns the current optimized transforms for the given 'trajectory_id'. diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c320215..b3d1b05 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -335,6 +335,15 @@ void SparsePoseGraph::WaitForAllComputations() { locker.Await([¬ification]() { return notification; }); } +void SparsePoseGraph::AddTrimmer( + std::unique_ptr trimmer) { + common::MutexLocker locker(&mutex_); + // C++11 does not allow us to move a unique_ptr into a lambda. + mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); + AddWorkItem([this, trimmer_ptr]() + REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); }); +} + void SparsePoseGraph::RunFinalOptimization() { WaitForAllComputations(); optimization_problem_.SetMaxNumIterations( @@ -386,6 +395,11 @@ void SparsePoseGraph::RunOptimization() { reverse_connected_components_.emplace(trajectory_id, i); } } + + TrimmingImplementation trimming(this); + for (auto& trimmer : trimmers_) { + trimmer->Trim(&trimming); + } } std::vector> @@ -468,5 +482,19 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( return result; } +SparsePoseGraph::TrimmingImplementation::TrimmingImplementation( + SparsePoseGraph* const parent) + : parent_(parent) {} + +int SparsePoseGraph::TrimmingImplementation::num_submaps( + const int trajectory_id) const { + return parent_->optimization_problem_.submap_data().at(trajectory_id).size(); +} + +void SparsePoseGraph::TrimmingImplementation::MarkSubmapAsTrimmed( + const mapping::SubmapId& submap_id) { + LOG(FATAL) << "Not yet implemented."; +} + } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index a6e9b38..84bf2ef 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,7 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" @@ -77,6 +79,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); + void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms(int trajectory_id) @@ -202,6 +205,24 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current submap transforms used for displaying data. std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); + + // List of all trimmers to consult when optimizations finish. + std::vector> trimmers_ + GUARDED_BY(mutex_); + + // Used to decide which submaps to trim. The 'mutex_' of the pose graph is + // held while this class is used. + class TrimmingImplementation : public mapping::TrimmingInterface { + public: + TrimmingImplementation(SparsePoseGraph* parent); + ~TrimmingImplementation() override {} + + int num_submaps(int trajectory_id) const override; + void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override; + + private: + SparsePoseGraph* const parent_; + }; }; } // namespace mapping_2d diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 1075ac6..9291fb9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -76,6 +76,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); + void AddTrimmer(std::unique_ptr trimmer) override { + LOG(FATAL) << "Not yet implemented for 3D."; + } void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms(int trajectory_id)