00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/2d/tsdf_2d.h"
00018
00019 #include <random>
00020
00021 #include "cartographer/mapping/value_conversion_tables.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(TSDF2DTest, ProtoConstructor) {
00032 proto::Grid2D proto;
00033 const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
00034 *proto.mutable_limits() = ToProto(limits);
00035 proto.mutable_tsdf_2d()->set_max_weight(10.0);
00036 proto.mutable_tsdf_2d()->set_truncation_distance(1.0);
00037 for (int i = 6; i < 12; ++i) {
00038 proto.mutable_cells()->Add(static_cast<uint16>(i));
00039 proto.mutable_tsdf_2d()->mutable_weight_cells()->Add(
00040 static_cast<uint16>(i));
00041 }
00042 proto.mutable_known_cells_box()->set_max_x(19);
00043 proto.mutable_known_cells_box()->set_max_y(20);
00044 proto.mutable_known_cells_box()->set_min_x(21);
00045 proto.mutable_known_cells_box()->set_min_y(22);
00046 proto.set_max_correspondence_cost(1.0);
00047 proto.set_min_correspondence_cost(-1.0);
00048
00049 ValueConversionTables conversion_tables;
00050 TSDF2D grid(proto, &conversion_tables);
00051 EXPECT_EQ(grid.GetGridType(), GridType::TSDF);
00052 EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
00053 }
00054
00055 TEST(TSDF2DTest, ConstructorGridType) {
00056 ValueConversionTables conversion_tables;
00057 TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,
00058 10.0f, &conversion_tables);
00059 EXPECT_EQ(tsdf.GetGridType(), GridType::TSDF);
00060 }
00061
00062 TEST(TSDF2DTest, ToProto) {
00063 ValueConversionTables conversion_tables;
00064 TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,
00065 10.0f, &conversion_tables);
00066
00067 const auto proto = tsdf.ToProto();
00068 EXPECT_EQ(ToProto(tsdf.limits()).DebugString(), proto.limits().DebugString());
00069 }
00070
00071 TEST(TSDF2DTest, GetCellIndex) {
00072 ValueConversionTables conversion_tables;
00073 TSDF2D tsdf(MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)), 1.f,
00074 10.f, &conversion_tables);
00075
00076 const MapLimits& limits = tsdf.limits();
00077 const CellLimits& cell_limits = limits.cell_limits();
00078 ASSERT_EQ(14, cell_limits.num_x_cells);
00079 ASSERT_EQ(8, cell_limits.num_y_cells);
00080 EXPECT_TRUE(
00081 (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all());
00082 EXPECT_TRUE(
00083 (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all());
00084 EXPECT_TRUE(
00085 (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all());
00086 EXPECT_TRUE(
00087 (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all());
00088
00089
00090 EXPECT_TRUE(
00091 (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all());
00092 EXPECT_TRUE(
00093 (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all());
00094 EXPECT_TRUE(
00095 (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all());
00096 EXPECT_TRUE(
00097 (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all());
00098 EXPECT_TRUE(
00099 (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all());
00100 }
00101
00102 TEST(TSDF2DTest, WriteRead) {
00103 const float truncation_distance = 1.f;
00104 const float max_weight = 10.f;
00105 ValueConversionTables conversion_tables;
00106 TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)),
00107 truncation_distance, max_weight, &conversion_tables);
00108
00109 const MapLimits& limits = tsdf.limits();
00110 EXPECT_EQ(1., limits.max().x());
00111 EXPECT_EQ(2., limits.max().y());
00112
00113 const CellLimits& cell_limits = limits.cell_limits();
00114 ASSERT_EQ(2, cell_limits.num_x_cells);
00115 ASSERT_EQ(2, cell_limits.num_y_cells);
00116
00117 std::mt19937 rng(42);
00118 std::uniform_real_distribution<float> tsd_distribution(-truncation_distance,
00119 truncation_distance);
00120 std::uniform_real_distribution<float> weight_distribution(0.f, max_weight);
00121 for (size_t i = 0; i < 1; ++i) {
00122 const float tsd = tsd_distribution(rng);
00123 const float weight = weight_distribution(rng);
00124 tsdf.SetCell(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)), tsd, weight);
00125 EXPECT_NEAR(
00126 tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).first,
00127 tsd, 2.f * truncation_distance / 32768.f);
00128 EXPECT_NEAR(
00129 tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).second,
00130 weight, max_weight / 32768.f);
00131 EXPECT_NEAR(tsdf.GetTSD(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), tsd,
00132 2.f * truncation_distance / 32768.f);
00133 EXPECT_NEAR(tsdf.GetWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))),
00134 weight, max_weight / 32768.f);
00135 }
00136 for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)),
00137 limits.GetCellIndex(Vector2f(0.5f, 0.5f)),
00138 limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) {
00139 EXPECT_TRUE(limits.Contains(xy_index));
00140 EXPECT_FALSE(tsdf.IsKnown(xy_index));
00141 }
00142 }
00143
00144 TEST(TSDF2DTest, CorrectCropping) {
00145
00146 const float truncation_distance = 1.f;
00147 const float max_weight = 10.f;
00148 std::mt19937 rng(42);
00149 std::uniform_real_distribution<float> tsdf_distribution(-truncation_distance,
00150 truncation_distance);
00151 std::uniform_real_distribution<float> weight_distribution(0.f, max_weight);
00152 ValueConversionTables conversion_tables;
00153 TSDF2D tsdf(MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)),
00154 truncation_distance, max_weight, &conversion_tables);
00155 for (const Array2i& xy_index :
00156 XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
00157 tsdf.SetCell(xy_index, tsdf_distribution(rng), weight_distribution(rng));
00158 }
00159 Array2i offset;
00160 CellLimits limits;
00161 tsdf.ComputeCroppedLimits(&offset, &limits);
00162 EXPECT_TRUE((offset == Array2i(100, 100)).all());
00163 EXPECT_EQ(limits.num_x_cells, 200);
00164 EXPECT_EQ(limits.num_y_cells, 200);
00165 }
00166
00167 }
00168 }
00169 }