00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/2d/probability_grid.h"
00018
00019 #include <random>
00020
00021 #include "cartographer/mapping/probability_values.h"
00022 #include "gtest/gtest.h"
00023
00024 namespace cartographer {
00025 namespace mapping {
00026 namespace {
00027
00028 using Eigen::Array2i;
00029 using Eigen::Vector2f;
00030
00031 TEST(ProbabilityGridTest, ProtoConstructor) {
00032 proto::Grid2D proto;
00033 const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
00034 *proto.mutable_limits() = ToProto(limits);
00035 for (int i = 6; i < 12; ++i) {
00036 proto.mutable_cells()->Add(static_cast<uint16>(i));
00037 }
00038 proto.mutable_known_cells_box()->set_max_x(19);
00039 proto.mutable_known_cells_box()->set_max_y(20);
00040 proto.mutable_known_cells_box()->set_min_x(21);
00041 proto.mutable_known_cells_box()->set_min_y(22);
00042 proto.mutable_probability_grid_2d();
00043
00044 ValueConversionTables conversion_tables;
00045 ProbabilityGrid grid(proto, &conversion_tables);
00046 EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
00047 EXPECT_EQ(grid.GetGridType(), GridType::PROBABILITY_GRID);
00048
00049
00050
00051 }
00052
00053 TEST(ProbabilityGridTest, ConstructorGridType) {
00054 ValueConversionTables conversion_tables;
00055 ProbabilityGrid probability_grid(
00056 MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
00057 &conversion_tables);
00058 EXPECT_EQ(probability_grid.GetGridType(), GridType::PROBABILITY_GRID);
00059 }
00060
00061 TEST(ProbabilityGridTest, ToProto) {
00062 ValueConversionTables conversion_tables;
00063 ProbabilityGrid probability_grid(
00064 MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
00065 &conversion_tables);
00066
00067 const auto proto = probability_grid.ToProto();
00068 EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(),
00069 proto.limits().DebugString());
00070
00071
00072
00073 }
00074
00075 TEST(ProbabilityGridTest, ApplyOdds) {
00076 ValueConversionTables conversion_tables;
00077 ProbabilityGrid probability_grid(
00078 MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
00079 &conversion_tables);
00080 const MapLimits& limits = probability_grid.limits();
00081
00082 EXPECT_TRUE(limits.Contains(Array2i(0, 0)));
00083 EXPECT_TRUE(limits.Contains(Array2i(0, 1)));
00084 EXPECT_TRUE(limits.Contains(Array2i(1, 0)));
00085 EXPECT_TRUE(limits.Contains(Array2i(1, 1)));
00086 EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 0)));
00087 EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 1)));
00088 EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 0)));
00089 EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 1)));
00090
00091 probability_grid.SetProbability(Array2i(1, 0), 0.5);
00092
00093 probability_grid.ApplyLookupTable(
00094 Array2i(1, 0),
00095 ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
00096 probability_grid.FinishUpdate();
00097 EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5);
00098
00099 probability_grid.SetProbability(Array2i(0, 1), 0.5);
00100 probability_grid.ApplyLookupTable(
00101 Array2i(0, 1),
00102 ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.1)));
00103 probability_grid.FinishUpdate();
00104 EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5);
00105
00106
00107 probability_grid.ApplyLookupTable(
00108 Array2i(1, 1),
00109 ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.42)));
00110 EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
00111
00112
00113 probability_grid.ApplyLookupTable(
00114 Array2i(1, 1),
00115 ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
00116 EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
00117 probability_grid.FinishUpdate();
00118 probability_grid.ApplyLookupTable(
00119 Array2i(1, 1),
00120 ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
00121 EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42);
00122 }
00123
00124 TEST(ProbabilityGridTest, GetProbability) {
00125 ValueConversionTables conversion_tables;
00126 ProbabilityGrid probability_grid(
00127 MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)),
00128 &conversion_tables);
00129
00130 const MapLimits& limits = probability_grid.limits();
00131 EXPECT_EQ(1., limits.max().x());
00132 EXPECT_EQ(2., limits.max().y());
00133
00134 const CellLimits& cell_limits = limits.cell_limits();
00135 ASSERT_EQ(2, cell_limits.num_x_cells);
00136 ASSERT_EQ(2, cell_limits.num_y_cells);
00137
00138 probability_grid.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)),
00139 kMaxProbability);
00140 EXPECT_NEAR(probability_grid.GetProbability(
00141 limits.GetCellIndex(Vector2f(-0.5f, 0.5f))),
00142 kMaxProbability, 1e-6);
00143 for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)),
00144 limits.GetCellIndex(Vector2f(0.5f, 0.5f)),
00145 limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) {
00146 EXPECT_TRUE(limits.Contains(xy_index));
00147 EXPECT_FALSE(probability_grid.IsKnown(xy_index));
00148 }
00149 }
00150
00151 TEST(ProbabilityGridTest, GetCellIndex) {
00152 ValueConversionTables conversion_tables;
00153 ProbabilityGrid probability_grid(
00154 MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)),
00155 &conversion_tables);
00156
00157 const MapLimits& limits = probability_grid.limits();
00158 const CellLimits& cell_limits = limits.cell_limits();
00159 ASSERT_EQ(14, cell_limits.num_x_cells);
00160 ASSERT_EQ(8, cell_limits.num_y_cells);
00161 EXPECT_TRUE(
00162 (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all());
00163 EXPECT_TRUE(
00164 (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all());
00165 EXPECT_TRUE(
00166 (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all());
00167 EXPECT_TRUE(
00168 (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all());
00169
00170
00171 EXPECT_TRUE(
00172 (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all());
00173 EXPECT_TRUE(
00174 (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all());
00175 EXPECT_TRUE(
00176 (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all());
00177 EXPECT_TRUE(
00178 (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all());
00179 EXPECT_TRUE(
00180 (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all());
00181 }
00182
00183 TEST(ProbabilityGridTest, CorrectCropping) {
00184
00185 std::mt19937 rng(42);
00186 std::uniform_real_distribution<float> value_distribution(kMinProbability,
00187 kMaxProbability);
00188 ValueConversionTables conversion_tables;
00189 ProbabilityGrid probability_grid(
00190 MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)),
00191 &conversion_tables);
00192 for (const Array2i& xy_index :
00193 XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
00194 probability_grid.SetProbability(xy_index, value_distribution(rng));
00195 }
00196 Array2i offset;
00197 CellLimits limits;
00198 probability_grid.ComputeCroppedLimits(&offset, &limits);
00199 EXPECT_TRUE((offset == Array2i(100, 100)).all());
00200 EXPECT_EQ(limits.num_x_cells, 200);
00201 EXPECT_EQ(limits.num_y_cells, 200);
00202 }
00203
00204 }
00205 }
00206 }