00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/2d/submap_2d.h"
00018
00019 #include <map>
00020 #include <memory>
00021 #include <set>
00022 #include <string>
00023
00024 #include "cartographer/common/lua_parameter_dictionary.h"
00025 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00026 #include "cartographer/common/port.h"
00027 #include "cartographer/mapping/2d/probability_grid.h"
00028 #include "cartographer/transform/transform.h"
00029 #include "gmock/gmock.h"
00030
00031 namespace cartographer {
00032 namespace mapping {
00033 namespace {
00034
00035 TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
00036 constexpr int kNumRangeData = 10;
00037 auto parameter_dictionary = common::MakeDictionary(
00038 "return {"
00039 "num_range_data = " +
00040 std::to_string(kNumRangeData) +
00041 ", "
00042 "grid_options_2d = {"
00043 "grid_type = \"PROBABILITY_GRID\","
00044 "resolution = 0.05, "
00045 "},"
00046 "range_data_inserter = {"
00047 "range_data_inserter_type = \"PROBABILITY_GRID_INSERTER_2D\","
00048 "probability_grid_range_data_inserter = {"
00049 "insert_free_space = true, "
00050 "hit_probability = 0.53, "
00051 "miss_probability = 0.495, "
00052 "},"
00053 "tsdf_range_data_inserter = { "
00054 "truncation_distance = 2.0,"
00055 "maximum_weight = 10.,"
00056 "update_free_space = false,"
00057 "normal_estimation_options = {"
00058 "num_normal_samples = 2,"
00059 "sample_radius = 10.,"
00060 "},"
00061 "project_sdf_distance_to_scan_normal = false,"
00062 "update_weight_range_exponent = 0,"
00063 "update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0,"
00064 "update_weight_distance_cell_to_hit_kernel_bandwidth = 0,"
00065 "},"
00066 "},"
00067 "}");
00068 ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())};
00069 std::set<std::shared_ptr<const Submap2D>> all_submaps;
00070 for (int i = 0; i != 1000; ++i) {
00071 auto insertion_submaps =
00072 submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}});
00073
00074
00075 for (const auto& submap : insertion_submaps) {
00076 all_submaps.insert(submap);
00077 }
00078 if (submaps.submaps().size() > 1) {
00079 EXPECT_LE(kNumRangeData, submaps.submaps().front()->num_range_data());
00080 }
00081 }
00082 EXPECT_EQ(2, submaps.submaps().size());
00083 int correct_num_finished_submaps = 0;
00084 int num_unfinished_submaps = 0;
00085 for (const auto& submap : all_submaps) {
00086 if (submap->num_range_data() == kNumRangeData * 2) {
00087 ++correct_num_finished_submaps;
00088 } else {
00089 EXPECT_EQ(kNumRangeData, submap->num_range_data());
00090 ++num_unfinished_submaps;
00091 }
00092 }
00093
00094 EXPECT_EQ(correct_num_finished_submaps, all_submaps.size() - 1);
00095 EXPECT_EQ(1, num_unfinished_submaps);
00096 }
00097
00098 TEST(Submap2DTest, ToFromProto) {
00099 MapLimits expected_map_limits(1., Eigen::Vector2d(2., 3.),
00100 CellLimits(100, 110));
00101 ValueConversionTables conversion_tables;
00102 Submap2D expected(Eigen::Vector2f(4.f, 5.f),
00103 absl::make_unique<ProbabilityGrid>(expected_map_limits,
00104 &conversion_tables),
00105 &conversion_tables);
00106 const proto::Submap proto =
00107 expected.ToProto(true );
00108 EXPECT_TRUE(proto.has_submap_2d());
00109 EXPECT_FALSE(proto.has_submap_3d());
00110 const auto actual = Submap2D(proto.submap_2d(), &conversion_tables);
00111 EXPECT_TRUE(expected.local_pose().translation().isApprox(
00112 actual.local_pose().translation(), 1e-6));
00113 EXPECT_TRUE(expected.local_pose().rotation().isApprox(
00114 actual.local_pose().rotation(), 1e-6));
00115 EXPECT_EQ(expected.num_range_data(), actual.num_range_data());
00116 EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished());
00117 EXPECT_NEAR(expected.grid()->limits().resolution(),
00118 actual.grid()->limits().resolution(), 1e-6);
00119 EXPECT_TRUE(expected.grid()->limits().max().isApprox(
00120 actual.grid()->limits().max(), 1e-6));
00121 EXPECT_EQ(expected.grid()->limits().cell_limits().num_x_cells,
00122 actual.grid()->limits().cell_limits().num_x_cells);
00123 }
00124
00125 }
00126 }
00127 }