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

@@ -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_

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 <memory>
 #include <set>
 #include <unordered_map>
 #include <utility>
@@ -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<PoseGraphTrimmer> trimmer) = 0;
+
   // Computes optimized poses.
   virtual void RunFinalOptimization() = 0;
 
-  // Get the current trajectory clusters.
+  // Gets the current trajectory clusters.
   virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
 
   // Returns the current optimized transforms for the given 'trajectory_id'.

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -335,6 +335,15 @@ void SparsePoseGraph::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
+void SparsePoseGraph::AddTrimmer(
+    std::unique_ptr<mapping::PoseGraphTrimmer> 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<std::vector<mapping::TrajectoryNode>>
@@ -468,5 +482,19 @@ std::vector<transform::Rigid3d> 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

cartographer/mapping_2d/sparse_pose_graph.h

@@ -21,6 +21,7 @@
 #include <functional>
 #include <limits>
 #include <map>
+#include <memory>
 #include <unordered_map>
 #include <vector>
 
@@ -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<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
@@ -202,6 +205,24 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Current submap transforms used for displaying data.
   std::vector<std::vector<sparse_pose_graph::SubmapData>>
       optimized_submap_transforms_ GUARDED_BY(mutex_);
+
+  // List of all trimmers to consult when optimizations finish.
+  std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> 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

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<mapping::PoseGraphTrimmer> trimmer) override {
+    LOG(FATAL) << "Not yet implemented for 3D.";
+  }
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)