Transform submap cells to global frame correctly. (#1130)

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

@@ -30,30 +30,24 @@ class SubmapCoverageGrid2D {
   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) {}
+  SubmapCoverageGrid2D(const MapLimits& map_limits)
+      : offset_(map_limits.max()), resolution_(map_limits.resolution()) {}
 
   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))};
+    CellId cell_id{common::RoundToInt64((offset_(0) - point(0)) / resolution_),
+                   common::RoundToInt64((offset_(1) - point(1)) / resolution_)};
     cells_[cell_id].emplace_back(submap_id, time);
   }
 
   const std::map<CellId, StoredType>& cells() const { return cells_; }
 
  private:
-  const Eigen::Vector2d offset_;
+  Eigen::Vector2d offset_;
+  double resolution_;
   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->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.
@@ -78,14 +72,26 @@ std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
       LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
       continue;
     }
-    const transform::Rigid2d projected_submap_pose =
-        transform::Project2D(submap.data.pose);
+
+    const transform::Rigid3d& global_frame_from_submap_frame = submap.data.pose;
+    const transform::Rigid3d submap_frame_from_local_frame =
+        submap.data.submap->local_pose().inverse();
     for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
       const Eigen::Array2i index = xy_index + offset;
       if (!grid.IsKnown(index)) continue;
+
+      const transform::Rigid3d center_of_cell_in_local_frame =
+          transform::Rigid3d::Translation(Eigen::Vector3d(
+              grid.limits().max().x() -
+                  grid.limits().resolution() * (index.y() + 0.5),
+              grid.limits().max().y() -
+                  grid.limits().resolution() * (index.x() + 0.5),
+              0));
+
       const transform::Rigid2d center_of_cell_in_global_frame =
-          projected_submap_pose *
-          transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5});
+          transform::Project2D(global_frame_from_submap_frame *
+                               submap_frame_from_local_frame *
+                               center_of_cell_in_local_frame);
       coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(),
                               submap.id, freshness->second);
     }
@@ -182,7 +188,11 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
     return;
   }
 
-  SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data));
+  const MapLimits first_submap_map_limits =
+      std::static_pointer_cast<const Submap2D>(submap_data.begin()->data.submap)
+          ->grid()
+          ->limits();
+  SubmapCoverageGrid2D coverage_grid(first_submap_map_limits);
   const std::map<SubmapId, common::Time> submap_freshness =
       ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(),
                              pose_graph->GetConstraints());

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

@@ -39,13 +39,15 @@ 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);
+  void AddSquareSubmap(const Rigid2d& global_to_submap_frame,
+                       const Rigid2d& local_to_submap_frame,
+                       const Eigen::Vector2d& submap_corner, int submap_index,
+                       int num_cells, bool is_finished) {
     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);
+    *submap_2d.mutable_local_pose() =
+        transform::ToProto(transform::Embed3D(local_to_submap_frame));
 
     auto* grid = submap_2d.mutable_grid();
     for (int i = 0; i < num_cells * num_cells; ++i) {
@@ -54,8 +56,7 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
 
     auto* map_limits = grid->mutable_limits();
     map_limits->set_resolution(1.0);
-    *map_limits->mutable_max() =
-        transform::ToProto(Eigen::Vector2d(num_cells, num_cells));
+    *map_limits->mutable_max() = transform::ToProto(submap_corner);
     map_limits->mutable_cell_limits()->set_num_x_cells(num_cells);
     map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
 
@@ -68,7 +69,8 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
     grid->mutable_probability_grid_2d();
     fake_pose_graph_.mutable_submap_data()->Insert(
         {0 /* trajectory_id */, submap_index},
-        {std::make_shared<const Submap2D>(submap_2d), pose_3d});
+        {std::make_shared<const Submap2D>(submap_2d),
+         transform::Embed3D(global_to_submap_frame)});
   }
 
   void AddTrajectoryNode(int node_index, int64 timestamp) {
@@ -107,9 +109,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
 }
 
 TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
-  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  0 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
-  AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  1 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@@ -125,9 +133,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
 }
 
 TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
-  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  0 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
-  AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  1 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@@ -140,7 +154,10 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
   trimmer.Trim(&fake_pose_graph_);
   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
 
-  AddSquareSubmap(Rigid2d::Identity(), 2 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  2 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
   AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
   AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true);
@@ -150,9 +167,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
 }
 
 TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
-  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  0 /* submap_index */, 1 /* num_cells */,
                   false /* is_finished */);
-  AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  1 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@@ -167,9 +190,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
 }
 
 TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
-  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  0 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
-  AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  1 /* submap_index */, 1 /* num_cells */,
                   true /* is_finished */);
   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@@ -195,9 +224,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
 // The background submap should be trimmed, since it has only 3 cells
 // not-covered by another submap.
 TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
-  AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 2 /* num_cells */,
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  0 /* submap_index */, 2 /* num_cells */,
                   true /* is_finished */);
-  AddSquareSubmap(Rigid2d::Translation(Eigen::Vector2d(1., 1.)),
+  AddSquareSubmap(Rigid2d::Translation(
+                      Eigen::Vector2d(1., 1.)) /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
                   1 /* submap_index */, 2 /* num_cells */,
                   true /* is_finished */);
   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
@@ -213,6 +248,31 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
               ElementsAre(EqualsSubmapId({0, 0})));
 }
 
+TEST_F(OverlappingSubmapsTrimmer2DTest, TestTransformations) {
+  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
+                  Rigid2d::Identity() /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  0 /* submap_index */, 1 /* num_cells */,
+                  true /* is_finished */);
+  const Rigid2d transform(Eigen::Vector2d(1., 1.), M_PI / 2);
+  AddSquareSubmap(transform /* global_from_submap_frame */,
+                  transform /* local_from_submap_frame */,
+                  Eigen::Vector2d(1., 1.) /* submap corner */,
+                  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 */,
+                                      0 /* min_added_submaps_count */);
+  trimmer.Trim(&fake_pose_graph_);
+  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
+              ElementsAre(EqualsSubmapId({0, 0})));
+}
+
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer