Add overlapping submaps trimmer. (#1027)

Trims submaps that have less than 'min_covered_cells_count' cells not overlapped by at least 'fresh_submaps_count` submaps.

cartographer/mapping/id.h

@@ -331,6 +331,15 @@ class MapById {
                : 0;
   }
 
+  // Returns count of all elements.
+  size_t size() const {
+    size_t size = 0;
+    for (const auto& item : trajectories_) {
+      size += item.second.data_.size();
+    }
+    return size;
+  }
+
   // Returns Range object for range-based loops over the nodes of a trajectory.
   Range<ConstIterator> trajectory(const int trajectory_id) const {
     return Range<ConstIterator>(BeginOfTrajectory(trajectory_id),

cartographer/mapping/id_test.cc

@@ -60,10 +60,12 @@ TEST(IdTest, EmptyMapById) {
   EXPECT_FALSE(map_by_id.empty());
   map_by_id.Trim(id);
   EXPECT_TRUE(map_by_id.empty());
+  EXPECT_EQ(0, map_by_id.size());
 }
 
 TEST(IdTest, MapByIdIterator) {
   MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
+  EXPECT_EQ(4, map_by_id.size());
   EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data);
   EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
               map_by_id.EndOfTrajectory(7));

cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc

@@ -0,0 +1,203 @@
+/*
+ * Copyright 2018 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/internal/2d/overlapping_submaps_trimmer_2d.h"
+
+#include <algorithm>
+
+#include "cartographer/mapping/2d/submap_2d.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+class SubmapCoverageGrid2D {
+ public:
+  // Aliases for documentation only (no type-safety).
+  using CellId = std::pair<int64 /* x cells */, int64 /* y cells */>;
+  using StoredType = std::vector<std::pair<SubmapId, common::Time>>;
+
+  explicit SubmapCoverageGrid2D(const Eigen::Vector2d& offset)
+      : offset_(offset) {}
+
+  void AddPoint(const Eigen::Vector2d& point, const SubmapId& submap_id,
+                const common::Time& time) {
+    CellId cell_id{common::RoundToInt64(offset_(0) - point(0)),
+                   common::RoundToInt64(offset_(1) - point(1))};
+    cells_[cell_id].emplace_back(submap_id, time);
+  }
+
+  const std::map<CellId, StoredType>& cells() const { return cells_; }
+
+ private:
+  const Eigen::Vector2d offset_;
+  std::map<CellId, StoredType> cells_;
+};
+
+Eigen::Vector2d GetCornerOfFirstSubmap(
+    const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
+  auto submap_2d = std::static_pointer_cast<const Submap2D>(
+      submap_data.begin()->data.submap);
+  return submap_2d->probability_grid().limits().max();
+}
+
+// Iterates over every cell in a submap, transforms the center of the cell to
+// the global frame and then adds the submap id and the timestamp of the most
+// recent range data insertion into the global grid.
+std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
+    const std::map<SubmapId, common::Time>& submap_freshness,
+    const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
+    SubmapCoverageGrid2D* coverage_grid) {
+  std::set<SubmapId> all_submap_ids;
+
+  for (const auto& submap : submap_data) {
+    auto freshness = submap_freshness.find(submap.id);
+    if (freshness == submap_freshness.end()) continue;
+    if (!submap.data.submap->finished()) continue;
+    all_submap_ids.insert(submap.id);
+    const ProbabilityGrid& probability_grid =
+        std::static_pointer_cast<const Submap2D>(submap.data.submap)
+            ->probability_grid();
+    // Iterate over every cell in a submap.
+    Eigen::Array2i offset;
+    CellLimits cell_limits;
+    probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
+    if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
+      LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
+      continue;
+    }
+    const transform::Rigid2d projected_submap_pose =
+        transform::Project2D(submap.data.pose);
+    for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
+      const Eigen::Array2i index = xy_index + offset;
+      if (!probability_grid.IsKnown(index)) continue;
+      const transform::Rigid2d center_of_cell_in_global_frame =
+          projected_submap_pose *
+          transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5});
+      coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(),
+                              submap.id, freshness->second);
+    }
+  }
+  return all_submap_ids;
+}
+
+// Uses intra-submap constraints and trajectory node timestamps to identify time
+// of the last range data insertion to the submap.
+std::map<SubmapId, common::Time> ComputeSubmapFreshness(
+    const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
+    const MapById<NodeId, TrajectoryNode>& trajectory_nodes,
+    const std::vector<PoseGraphInterface::Constraint>& constraints) {
+  std::map<SubmapId, common::Time> submap_freshness;
+
+  // Find the node with the largest NodeId per SubmapId.
+  std::map<SubmapId, NodeId> submap_to_latest_node;
+  for (const PoseGraphInterface::Constraint& constraint : constraints) {
+    if (constraint.tag != PoseGraphInterface::Constraint::INTRA_SUBMAP) {
+      continue;
+    }
+    auto submap_to_node = submap_to_latest_node.find(constraint.submap_id);
+    if (submap_to_node == submap_to_latest_node.end()) {
+      submap_to_latest_node.insert(
+          std::make_pair(constraint.submap_id, constraint.node_id));
+      continue;
+    }
+    submap_to_node->second =
+        std::max(submap_to_node->second, constraint.node_id);
+  }
+
+  // Find timestamp of every latest node.
+  for (const auto& submap_id_to_node_id : submap_to_latest_node) {
+    auto submap_data_item = submap_data.find(submap_id_to_node_id.first);
+    if (submap_data_item == submap_data.end()) {
+      LOG(WARNING) << "Intra-submap constraint between SubmapID = "
+                   << submap_id_to_node_id.first << " and NodeID "
+                   << submap_id_to_node_id.second << " is missing submap data";
+      continue;
+    }
+    auto latest_node_id = trajectory_nodes.find(submap_id_to_node_id.second);
+    if (latest_node_id == trajectory_nodes.end()) continue;
+    submap_freshness[submap_data_item->id] = latest_node_id->data.time();
+  }
+  return submap_freshness;
+}
+
+// Returns IDs of submaps that have less than 'min_covered_cells_count' cells
+// not overlapped by at least 'fresh_submaps_count' submaps.
+std::vector<SubmapId> FindSubmapIdsToTrim(
+    const SubmapCoverageGrid2D& coverage_grid,
+    const std::set<SubmapId>& all_submap_ids, uint16 fresh_submaps_count,
+    uint16 min_covered_cells_count) {
+  std::map<SubmapId, uint16> cells_covered_by_submap;
+  for (const auto& cell : coverage_grid.cells()) {
+    std::vector<std::pair<SubmapId, common::Time>> submaps_per_cell(
+        cell.second);
+
+    // In case there are several submaps covering the cell, only the freshest
+    // submaps are kept.
+    if (submaps_per_cell.size() > fresh_submaps_count) {
+      // Sort by time in descending order.
+      std::sort(submaps_per_cell.begin(), submaps_per_cell.end(),
+                [](const std::pair<SubmapId, common::Time>& left,
+                   const std::pair<SubmapId, common::Time>& right) {
+                  return left.second > right.second;
+                });
+      submaps_per_cell.erase(submaps_per_cell.begin() + fresh_submaps_count,
+                             submaps_per_cell.end());
+    }
+    for (const std::pair<SubmapId, common::Time>& submap : submaps_per_cell) {
+      ++cells_covered_by_submap[submap.first];
+    }
+  }
+  std::vector<SubmapId> submap_ids_to_keep;
+  for (const auto& id_to_cells_count : cells_covered_by_submap) {
+    if (id_to_cells_count.second < min_covered_cells_count) continue;
+    submap_ids_to_keep.push_back(id_to_cells_count.first);
+  }
+
+  DCHECK(std::is_sorted(submap_ids_to_keep.begin(), submap_ids_to_keep.end()));
+
+  std::vector<SubmapId> result;
+  std::set_difference(all_submap_ids.begin(), all_submap_ids.end(),
+                      submap_ids_to_keep.begin(), submap_ids_to_keep.end(),
+                      std::inserter(result, result.begin()));
+  return result;
+}
+
+}  // namespace
+
+void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
+  const auto submap_data = pose_graph->GetAllSubmapData();
+  if (submap_data.size() == 0) return;
+
+  const auto constraints = pose_graph->GetConstraints();
+  const auto trajectory_nodes = pose_graph->GetTrajectoryNodes();
+
+  SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data));
+
+  const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
+      ComputeSubmapFreshness(submap_data, trajectory_nodes, constraints),
+      submap_data, &coverage_grid);
+  const std::vector<SubmapId> submap_ids_to_remove =
+      FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_,
+                          min_covered_cells_count_);
+  for (const SubmapId& id : submap_ids_to_remove) {
+    pose_graph->MarkSubmapAsTrimmed(id);
+  }
+  finished_ = true;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright 2018 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_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_
+
+#include "cartographer/common/port.h"
+#include "cartographer/mapping/pose_graph_trimmer.h"
+
+namespace cartographer {
+namespace mapping {
+
+// Trims submaps that have less than 'min_covered_cells_count' cells not
+// overlapped by at least 'fresh_submaps_count` submaps.
+class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
+ public:
+  OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count,
+                              uint16 min_covered_cells_count)
+      : fresh_submaps_count_(fresh_submaps_count),
+        min_covered_cells_count_(min_covered_cells_count) {}
+  ~OverlappingSubmapsTrimmer2D() override = default;
+
+  void Trim(Trimmable* pose_graph) override;
+  bool IsFinished() override { return finished_; }
+
+ private:
+  // Number of the most recent submaps to keep.
+  const uint16 fresh_submaps_count_;
+  // Minimal number of covered cells to keep submap from trimming.
+  const uint16 min_covered_cells_count_;
+
+  bool finished_ = false;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_

cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc

@@ -0,0 +1,187 @@
+/*
+ * Copyright 2018 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/internal/2d/overlapping_submaps_trimmer_2d.h"
+
+#include <vector>
+
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer/mapping/internal/testing/fake_trimmable.h"
+#include "cartographer/transform/transform.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+using ::cartographer::transform::Rigid2d;
+using ::cartographer::transform::Rigid3d;
+using ::testing::ElementsAre;
+using ::testing::Field;
+using ::testing::IsEmpty;
+
+class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
+ protected:
+  // Creates a submap with num_cells x num_cells grid.
+  void AddSquareSubmap(const Rigid2d& pose_2d, int submap_index, int num_cells,
+                       bool is_finished) {
+    const Rigid3d pose_3d = transform::Embed3D(pose_2d);
+    proto::Submap2D submap_2d;
+    submap_2d.set_num_range_data(1);
+    submap_2d.set_finished(is_finished);
+    *submap_2d.mutable_local_pose() = transform::ToProto(pose_3d);
+
+    auto* probability_grid = submap_2d.mutable_probability_grid();
+    for (int i = 0; i < num_cells * num_cells; ++i) {
+      probability_grid->add_cells(1);
+    }
+
+    auto* map_limits = probability_grid->mutable_limits();
+    map_limits->set_resolution(1.0);
+    *map_limits->mutable_max() =
+        transform::ToProto(Eigen::Vector2d(num_cells, num_cells));
+    map_limits->mutable_cell_limits()->set_num_x_cells(num_cells);
+    map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
+
+    auto* know_cells_box = probability_grid->mutable_known_cells_box();
+    know_cells_box->set_min_x(0);
+    know_cells_box->set_min_y(0);
+    know_cells_box->set_max_x(num_cells - 1);
+    know_cells_box->set_max_y(num_cells - 1);
+
+    fake_pose_graph_.mutable_submap_data()->Insert(
+        {0 /* trajectory_id */, submap_index},
+        {std::make_shared<const Submap2D>(submap_2d), pose_3d});
+  }
+
+  void AddTrajectoryNode(int node_index, int64 timestamp) {
+    TrajectoryNode::Data data;
+    data.time = common::FromUniversal(timestamp);
+
+    fake_pose_graph_.mutable_trajectory_nodes()->Insert(
+        NodeId{0 /* trajectory_id */, node_index},
+        {std::make_shared<const TrajectoryNode::Data>(data), {} /* pose */});
+  }
+
+  void AddConstraint(int submap_index, int node_index, bool is_intra_submap) {
+    fake_pose_graph_.mutable_constraints()->push_back(
+        {SubmapId{0 /* trajectory_id */, submap_index},
+         NodeId{0 /* trajectory_id */, node_index},
+         {} /* pose */,
+         is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP
+                         : PoseGraphInterface::Constraint::INTER_SUBMAP});
+  }
+
+  testing::FakeTrimmable fake_pose_graph_;
+};
+
+::testing::Matcher<const SubmapId&> EqualsSubmapId(const SubmapId& expected) {
+  return ::testing::AllOf(
+      Field(&SubmapId::trajectory_id, expected.trajectory_id),
+      Field(&SubmapId::submap_index, expected.submap_index));
+}
+
+TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
+  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
+                                      0 /* min_covered_cells_count */);
+  trimmer.Trim(&fake_pose_graph_);
+  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
+}
+
+TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
+  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
+                  true /* is_finished */);
+  AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
+                  true /* is_finished */);
+  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
+  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
+  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
+  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
+
+  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
+                                      0 /* min_covered_cells_count */);
+  trimmer.Trim(&fake_pose_graph_);
+  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
+              ElementsAre(EqualsSubmapId({0, 0})));
+}
+
+TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
+  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
+                  false /* is_finished */);
+  AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
+                  true /* is_finished */);
+  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
+  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
+  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
+  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
+
+  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
+                                      0 /* min_covered_cells_count */);
+  trimmer.Trim(&fake_pose_graph_);
+  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
+}
+
+TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
+  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
+                  true /* is_finished */);
+  AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
+                  true /* is_finished */);
+  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
+  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
+  AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
+  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, false);
+  AddConstraint(0 /*submap_index*/, 2 /*node_index*/, true);
+  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
+
+  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
+                                      0 /* min_covered_cells_count */);
+  trimmer.Trim(&fake_pose_graph_);
+  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
+              ElementsAre(EqualsSubmapId({0, 1})));
+}
+
+// This test covers two 4-cells submaps that overlap each other displaced like:
+//    ___
+//  _|   |
+// | |_ _|
+// |___|
+//
+// The background submap should be trimmed, since it has only 3 cells
+// not-covered by another submap.
+TEST_F(OverlappingSubmapsTrimmer2DTest, ) {
+  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 2 /* num_cells */,
+                  true /* is_finished */);
+  AddSquareSubmap(Rigid2d::Translation(Eigen::Vector2d(1., 1.)),
+                  1 /* submap_index */, 2 /* num_cells */,
+                  true /* is_finished */);
+  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
+  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
+  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
+  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
+
+  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
+                                      4 /* min_covered_cells_count */);
+  trimmer.Trim(&fake_pose_graph_);
+  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
+              ElementsAre(EqualsSubmapId({0, 0})));
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/testing/fake_trimmable.h

@@ -27,32 +27,66 @@ namespace testing {
 
 class FakeTrimmable : public Trimmable {
  public:
+  FakeTrimmable() = default;
+
+  // Populates dummy SubmapIDs.
   FakeTrimmable(int trajectory_id, int num_submaps) {
     for (int index = 0; index < num_submaps; ++index) {
-      submaps_.push_back(SubmapId{trajectory_id, index});
+      submap_data_.Insert(SubmapId{trajectory_id, index}, {});
     }
   }
   ~FakeTrimmable() override {}
 
   int num_submaps(const int trajectory_id) const override {
-    return submaps_.size() - trimmed_submaps_.size();
+    return submap_data_.size() - trimmed_submaps_.size();
   }
 
   std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override {
-    return submaps_;
+    std::vector<SubmapId> submap_ids;
+    for (const auto& submap : submap_data_) {
+      submap_ids.push_back(submap.id);
+    }
+    return submap_ids;
+  }
+
+  void set_submap_data(
+      const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
+    submap_data_ = submap_data;
+  }
+
+  MapById<SubmapId, PoseGraphInterface::SubmapData>* mutable_submap_data() {
+    return &submap_data_;
   }
 
   MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
       const override {
-    return {};
+    return submap_data_;
+  }
+
+  void set_trajectory_nodes(
+      const MapById<NodeId, TrajectoryNode>& trajectory_nodes) {
+    trajectory_nodes_ = trajectory_nodes;
+  }
+
+  MapById<NodeId, TrajectoryNode>* mutable_trajectory_nodes() {
+    return &trajectory_nodes_;
   }
 
   MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override {
-    return {};
+    return trajectory_nodes_;
+  }
+
+  void set_constraints(
+      const std::vector<PoseGraphInterface::Constraint>& constraints) {
+    constraints_ = constraints;
+  }
+
+  std::vector<PoseGraphInterface::Constraint>* mutable_constraints() {
+    return &constraints_;
   }
 
   std::vector<PoseGraphInterface::Constraint> GetConstraints() const override {
-    return {};
+    return constraints_;
   }
 
   void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
@@ -64,8 +98,11 @@ class FakeTrimmable : public Trimmable {
   std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
 
  private:
-  std::vector<SubmapId> submaps_;
   std::vector<SubmapId> trimmed_submaps_;
+
+  std::vector<PoseGraphInterface::Constraint> constraints_;
+  MapById<NodeId, TrajectoryNode> trajectory_nodes_;
+  MapById<SubmapId, PoseGraphInterface::SubmapData> submap_data_;
 };
 
 }  // namespace testing