cartographer: overlapping_submaps_trimmer_2d

Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
00018 
00019 #include <vector>
00020 
00021 #include "cartographer/common/time.h"
00022 #include "cartographer/mapping/2d/submap_2d.h"
00023 #include "cartographer/mapping/id.h"
00024 #include "cartographer/mapping/internal/testing/fake_trimmable.h"
00025 #include "cartographer/transform/transform.h"
00026 #include "gmock/gmock.h"
00027 #include "gtest/gtest.h"
00028 
00029 namespace cartographer {
00030 namespace mapping {
00031 namespace {
00032 
00033 using ::cartographer::transform::Rigid2d;
00034 using ::cartographer::transform::Rigid3d;
00035 using ::testing::ElementsAre;
00036 using ::testing::Field;
00037 using ::testing::IsEmpty;
00038 
00039 class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
00040  protected:
00041   // Creates a submap with num_cells x num_cells grid.
00042   void AddSquareSubmap(const Rigid2d& global_to_submap_frame,
00043                        const Rigid2d& local_to_submap_frame,
00044                        const Eigen::Vector2d& submap_corner, int submap_index,
00045                        int num_cells, bool is_finished) {
00046     proto::Submap2D submap_2d;
00047     submap_2d.set_num_range_data(1);
00048     submap_2d.set_finished(is_finished);
00049     *submap_2d.mutable_local_pose() =
00050         transform::ToProto(transform::Embed3D(local_to_submap_frame));
00051 
00052     auto* grid = submap_2d.mutable_grid();
00053     for (int i = 0; i < num_cells * num_cells; ++i) {
00054       grid->add_cells(1);
00055     }
00056 
00057     auto* map_limits = grid->mutable_limits();
00058     map_limits->set_resolution(1.0);
00059     *map_limits->mutable_max() = transform::ToProto(submap_corner);
00060     map_limits->mutable_cell_limits()->set_num_x_cells(num_cells);
00061     map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
00062 
00063     auto* know_cells_box = grid->mutable_known_cells_box();
00064     know_cells_box->set_min_x(0);
00065     know_cells_box->set_min_y(0);
00066     know_cells_box->set_max_x(num_cells - 1);
00067     know_cells_box->set_max_y(num_cells - 1);
00068 
00069     grid->mutable_probability_grid_2d();
00070     fake_pose_graph_.mutable_submap_data()->Insert(
00071         {0 /* trajectory_id */, submap_index},
00072         {std::make_shared<const Submap2D>(submap_2d, &conversion_tables_),
00073          transform::Embed3D(global_to_submap_frame)});
00074   }
00075 
00076   void AddTrajectoryNode(int node_index, int64 timestamp) {
00077     TrajectoryNode::Data data;
00078     data.time = common::FromUniversal(timestamp);
00079 
00080     fake_pose_graph_.mutable_trajectory_nodes()->Insert(
00081         NodeId{0 /* trajectory_id */, node_index},
00082         {std::make_shared<const TrajectoryNode::Data>(data), {} /* pose */});
00083   }
00084 
00085   void AddConstraint(int submap_index, int node_index, bool is_intra_submap) {
00086     fake_pose_graph_.mutable_constraints()->push_back(
00087         {SubmapId{0 /* trajectory_id */, submap_index},
00088          NodeId{0 /* trajectory_id */, node_index},
00089          {} /* pose */,
00090          is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP
00091                          : PoseGraphInterface::Constraint::INTER_SUBMAP});
00092   }
00093 
00094   ValueConversionTables conversion_tables_;
00095   testing::FakeTrimmable fake_pose_graph_;
00096 };
00097 
00098 ::testing::Matcher<const SubmapId&> EqualsSubmapId(const SubmapId& expected) {
00099   return ::testing::AllOf(
00100       Field(&SubmapId::trajectory_id, expected.trajectory_id),
00101       Field(&SubmapId::submap_index, expected.submap_index));
00102 }
00103 
00104 TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
00105   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
00106                                       0 /* min_covered_area */,
00107                                       0 /* min_added_submaps_count */);
00108   trimmer.Trim(&fake_pose_graph_);
00109   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
00110 }
00111 
00112 TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
00113   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00114                   Rigid2d::Identity() /* local_from_submap_frame */,
00115                   Eigen::Vector2d(1., 1.) /* submap corner */,
00116                   0 /* submap_index */, 1 /* num_cells */,
00117                   true /* is_finished */);
00118   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00119                   Rigid2d::Identity() /* local_from_submap_frame */,
00120                   Eigen::Vector2d(1., 1.) /* submap corner */,
00121                   1 /* submap_index */, 1 /* num_cells */,
00122                   true /* is_finished */);
00123   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
00124   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
00125   AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
00126   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
00127 
00128   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
00129                                       0 /* min_covered_area */,
00130                                       0 /* min_added_submaps_count */);
00131   trimmer.Trim(&fake_pose_graph_);
00132   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
00133               ElementsAre(EqualsSubmapId({0, 0})));
00134 }
00135 
00136 TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
00137   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00138                   Rigid2d::Identity() /* local_from_submap_frame */,
00139                   Eigen::Vector2d(1., 1.) /* submap corner */,
00140                   0 /* submap_index */, 1 /* num_cells */,
00141                   true /* is_finished */);
00142   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00143                   Rigid2d::Identity() /* local_from_submap_frame */,
00144                   Eigen::Vector2d(1., 1.) /* submap corner */,
00145                   1 /* submap_index */, 1 /* num_cells */,
00146                   true /* is_finished */);
00147   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
00148   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
00149   AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
00150   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
00151 
00152   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
00153                                       0 /* min_covered_area */,
00154                                       2 /* min_added_submaps_count */);
00155   trimmer.Trim(&fake_pose_graph_);
00156   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
00157 
00158   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00159                   Rigid2d::Identity() /* local_from_submap_frame */,
00160                   Eigen::Vector2d(1., 1.) /* submap corner */,
00161                   2 /* submap_index */, 1 /* num_cells */,
00162                   true /* is_finished */);
00163   AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
00164   AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true);
00165   trimmer.Trim(&fake_pose_graph_);
00166   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
00167               ElementsAre(EqualsSubmapId({0, 0}), EqualsSubmapId({0, 1})));
00168 }
00169 
00170 TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
00171   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00172                   Rigid2d::Identity() /* local_from_submap_frame */,
00173                   Eigen::Vector2d(1., 1.) /* submap corner */,
00174                   0 /* submap_index */, 1 /* num_cells */,
00175                   false /* is_finished */);
00176   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00177                   Rigid2d::Identity() /* local_from_submap_frame */,
00178                   Eigen::Vector2d(1., 1.) /* submap corner */,
00179                   1 /* submap_index */, 1 /* num_cells */,
00180                   true /* is_finished */);
00181   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
00182   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
00183   AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
00184   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
00185 
00186   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
00187                                       0 /* min_covered_area */,
00188                                       0 /* min_added_submaps_count */);
00189   trimmer.Trim(&fake_pose_graph_);
00190   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
00191 }
00192 
00193 TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
00194   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00195                   Rigid2d::Identity() /* local_from_submap_frame */,
00196                   Eigen::Vector2d(1., 1.) /* submap corner */,
00197                   0 /* submap_index */, 1 /* num_cells */,
00198                   true /* is_finished */);
00199   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00200                   Rigid2d::Identity() /* local_from_submap_frame */,
00201                   Eigen::Vector2d(1., 1.) /* submap corner */,
00202                   1 /* submap_index */, 1 /* num_cells */,
00203                   true /* is_finished */);
00204   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
00205   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
00206   AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
00207   AddConstraint(0 /*submap_index*/, 0 /*node_index*/, false);
00208   AddConstraint(0 /*submap_index*/, 2 /*node_index*/, true);
00209   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
00210 
00211   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
00212                                       0 /* min_covered_area */,
00213                                       0 /* min_added_submaps_count */);
00214   trimmer.Trim(&fake_pose_graph_);
00215   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
00216               ElementsAre(EqualsSubmapId({0, 1})));
00217 }
00218 
00219 // This test covers two 4-cells submaps that overlap each other displaced like:
00220 //    ___
00221 //  _|   |
00222 // | |_ _|
00223 // |___|
00224 //
00225 // The background submap should be trimmed, since it has only 3 cells
00226 // not-covered by another submap.
00227 TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
00228   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00229                   Rigid2d::Identity() /* local_from_submap_frame */,
00230                   Eigen::Vector2d(1., 1.) /* submap corner */,
00231                   0 /* submap_index */, 2 /* num_cells */,
00232                   true /* is_finished */);
00233   AddSquareSubmap(Rigid2d::Translation(
00234                       Eigen::Vector2d(1., 1.)) /* global_from_submap_frame */,
00235                   Rigid2d::Identity() /* local_from_submap_frame */,
00236                   Eigen::Vector2d(1., 1.) /* submap corner */,
00237                   1 /* submap_index */, 2 /* num_cells */,
00238                   true /* is_finished */);
00239   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
00240   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
00241   AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
00242   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
00243 
00244   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
00245                                       4 /* min_covered_cells_count */,
00246                                       0 /* min_added_submaps_count */);
00247   trimmer.Trim(&fake_pose_graph_);
00248   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
00249               ElementsAre(EqualsSubmapId({0, 0})));
00250 }
00251 
00252 TEST_F(OverlappingSubmapsTrimmer2DTest, TestTransformations) {
00253   AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
00254                   Rigid2d::Identity() /* local_from_submap_frame */,
00255                   Eigen::Vector2d(1., 1.) /* submap corner */,
00256                   0 /* submap_index */, 1 /* num_cells */,
00257                   true /* is_finished */);
00258   const Rigid2d transform(Eigen::Vector2d(1., 1.), M_PI / 2);
00259   AddSquareSubmap(transform /* global_from_submap_frame */,
00260                   transform /* local_from_submap_frame */,
00261                   Eigen::Vector2d(1., 1.) /* submap corner */,
00262                   1 /* submap_index */, 1 /* num_cells */,
00263                   true /* is_finished */);
00264   AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
00265   AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
00266   AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
00267   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
00268 
00269   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
00270                                       0 /* min_covered_area */,
00271                                       0 /* min_added_submaps_count */);
00272   trimmer.Trim(&fake_pose_graph_);
00273   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
00274               ElementsAre(EqualsSubmapId({0, 0})));
00275 }
00276 
00277 }  // namespace
00278 }  // namespace mapping
00279 }  // namespace cartographer