Adds a PureLocalizationTrimmer. (#316)

Related to #315.

cartographer/mapping/id.h

@@ -47,6 +47,11 @@ struct SubmapId {
   int trajectory_id;
   int submap_index;
 
+  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);
+  }
+
   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);

cartographer/mapping/pose_graph_trimmer.cc

@@ -0,0 +1,41 @@
+/*
+ * 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.
+ */
+
+#include "cartographer/mapping/pose_graph_trimmer.h"
+
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
+                                                 const int num_submaps_to_keep)
+    : trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) {
+  CHECK_GE(num_submaps_to_keep, 3);
+}
+
+void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
+  const int total_num_submaps = pose_graph->num_submaps(trajectory_id_);
+  while (total_num_submaps > num_submaps_trimmed_ + num_submaps_to_keep_) {
+    const int submap_index_to_trim_next = num_submaps_trimmed_;
+    pose_graph->MarkSubmapAsTrimmed(
+        SubmapId{trajectory_id_, submap_index_to_trim_next});
+    ++num_submaps_trimmed_;
+  }
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/pose_graph_trimmer.h

@@ -24,9 +24,9 @@ namespace mapping {
 
 // Implemented by the pose graph to provide thread-safe access to functions for
 // trimming the graph.
-class TrimmingInterface {
+class Trimmable {
  public:
-  virtual ~TrimmingInterface() {}
+  virtual ~Trimmable() {}
 
   // TODO(whess): This is all the functionality necessary for pure localization.
   // To be expanded as needed for lifelong mapping.
@@ -44,7 +44,22 @@ class PoseGraphTrimmer {
   virtual ~PoseGraphTrimmer() {}
 
   // Called once after each pose graph optimization.
-  virtual void Trim(TrimmingInterface* trimming) = 0;
+  virtual void Trim(Trimmable* pose_graph) = 0;
+};
+
+// Keeps the last 'num_submaps_to_keep' of the trajectory with 'trajectory_id'
+// to implement localization without mapping.
+class PureLocalizationTrimmer : public PoseGraphTrimmer {
+ public:
+  PureLocalizationTrimmer(int trajectory_id, int num_submaps_to_keep);
+  ~PureLocalizationTrimmer() override {}
+
+  void Trim(Trimmable* pose_graph) override;
+
+ private:
+  const int trajectory_id_;
+  const int num_submaps_to_keep_;
+  int num_submaps_trimmed_ = 0;
 };
 
 }  // namespace mapping

cartographer/mapping/pose_graph_trimmer_test.cc

@@ -0,0 +1,58 @@
+/*
+ * 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.
+ */
+
+#include "cartographer/mapping/pose_graph_trimmer.h"
+
+#include <vector>
+
+#include "cartographer/mapping/id.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+class FakePoseGraph : public Trimmable {
+ public:
+  ~FakePoseGraph() override {}
+
+  int num_submaps(int trajectory_id) const override { return 17; }
+
+  void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
+    trimmed_submaps_.push_back(submap_id);
+  }
+
+  std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
+
+ private:
+  std::vector<SubmapId> trimmed_submaps_;
+};
+
+TEST(PureLocalizationTrimmerTest, MarksSubmapsAsExpected) {
+  const int kTrajectoryId = 42;
+  PureLocalizationTrimmer trimmer(kTrajectoryId, 15);
+  FakePoseGraph fake_pose_graph;
+  trimmer.Trim(&fake_pose_graph);
+
+  const auto trimmed_submaps = fake_pose_graph.trimmed_submaps();
+  ASSERT_EQ(2, trimmed_submaps.size());
+  EXPECT_EQ((SubmapId{kTrajectoryId, 0}), trimmed_submaps[0]);
+  EXPECT_EQ((SubmapId{kTrajectoryId, 1}), trimmed_submaps[1]);
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/probability_values_test.cc

@@ -15,6 +15,7 @@
  */
 
 #include "cartographer/mapping/probability_values.h"
+
 #include "gtest/gtest.h"
 
 namespace cartographer {

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -396,9 +396,9 @@ void SparsePoseGraph::RunOptimization() {
     }
   }
 
-  TrimmingImplementation trimming(this);
+  TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {
-    trimmer->Trim(&trimming);
+    trimmer->Trim(&trimming_handle);
   }
 }
 
@@ -482,16 +482,15 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
   return result;
 }
 
-SparsePoseGraph::TrimmingImplementation::TrimmingImplementation(
+SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
-    SparsePoseGraph* const parent)
     : parent_(parent) {}
 
-int SparsePoseGraph::TrimmingImplementation::num_submaps(
+int SparsePoseGraph::TrimmingHandle::num_submaps(
     const int trajectory_id) const {
   return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
 }
 
-void SparsePoseGraph::TrimmingImplementation::MarkSubmapAsTrimmed(
+void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
     const mapping::SubmapId& submap_id) {
   LOG(FATAL) << "Not yet implemented.";
 }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -210,12 +210,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   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
+  // Allows querying and manipulating the pose graph by the 'trimmers_'. The
-  // held while this class is used.
+  // 'mutex_' of the pose graph is held while this class is used.
-  class TrimmingImplementation : public mapping::TrimmingInterface {
+  class TrimmingHandle : public mapping::Trimmable {
    public:
-    TrimmingImplementation(SparsePoseGraph* parent);
+    TrimmingHandle(SparsePoseGraph* parent);
-    ~TrimmingImplementation() override {}
+    ~TrimmingHandle() override {}
 
     int num_submaps(int trajectory_id) const override;
     void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override;