From ff18bae528846e6f4cfe81a711ea9e196878f356 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Mon, 30 Apr 2018 12:12:48 +0200 Subject: [PATCH] Transform submap cells to global frame correctly. (#1130) --- .../2d/overlapping_submaps_trimmer_2d.cc | 44 +++++---- .../2d/overlapping_submaps_trimmer_2d_test.cc | 96 +++++++++++++++---- 2 files changed, 105 insertions(+), 35 deletions(-) diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index 11c8e5e..5314aa6 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -30,30 +30,24 @@ class SubmapCoverageGrid2D { using CellId = std::pair; using StoredType = std::vector>; - 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& cells() const { return cells_; } private: - const Eigen::Vector2d offset_; + Eigen::Vector2d offset_; + double resolution_; std::map cells_; }; -Eigen::Vector2d GetCornerOfFirstSubmap( - const MapById& submap_data) { - auto submap_2d = std::static_pointer_cast( - 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 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(submap_data.begin()->data.submap) + ->grid() + ->limits(); + SubmapCoverageGrid2D coverage_grid(first_submap_map_limits); const std::map submap_freshness = ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(), pose_graph->GetConstraints()); diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc index ef2689c..7e5c90c 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc +++ b/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(submap_2d), pose_3d}); + {std::make_shared(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