diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc index d64d897..f726a2f 100644 --- a/cartographer/mapping/2d/grid_2d.cc +++ b/cartographer/mapping/2d/grid_2d.cc @@ -63,6 +63,7 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost, float max_correspondence_cost, ValueConversionTables* conversion_tables) : limits_(limits), + grid_type_(GridType::NONE), correspondence_cost_cells_( limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, kUnknownCorrespondenceValue), @@ -77,6 +78,7 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost, Grid2D::Grid2D(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : limits_(proto.limits()), + grid_type_(GridType::NONE), correspondence_cost_cells_(), min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)), max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)), diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index 89ad674..7fdb35d 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -32,6 +32,8 @@ namespace mapping { proto::GridOptions2D CreateGridOptions2D( common::LuaParameterDictionary* const parameter_dictionary); +enum class GridType { NONE, PROBABILITY_GRID, TSDF }; + class Grid2D : public GridInterface { public: Grid2D(const MapLimits& limits, float min_correspondence_cost, @@ -43,6 +45,8 @@ class Grid2D : public GridInterface { // Returns the limits of this Grid2D. const MapLimits& limits() const { return limits_; } + GridType grid_type() const { return grid_type_; } + // Finishes the update sequence. void FinishUpdate(); // Returns the correspondence cost of the cell with 'cell_index'. @@ -91,6 +95,8 @@ class Grid2D : public GridInterface { std::vector* mutable_correspondence_cost_cells() { return &correspondence_cost_cells_; } + + GridType* mutable_grid_type() { return &grid_type_; } std::vector* mutable_update_indices() { return &update_indices_; } Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } @@ -99,6 +105,7 @@ class Grid2D : public GridInterface { private: MapLimits limits_; + GridType grid_type_; std::vector correspondence_cost_cells_; float min_correspondence_cost_; float max_correspondence_cost_; diff --git a/cartographer/mapping/2d/probability_grid.cc b/cartographer/mapping/2d/probability_grid.cc index 5c954e9..85a9981 100644 --- a/cartographer/mapping/2d/probability_grid.cc +++ b/cartographer/mapping/2d/probability_grid.cc @@ -28,12 +28,15 @@ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits, ValueConversionTables* conversion_tables) : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost, conversion_tables), - conversion_tables_(conversion_tables) {} + conversion_tables_(conversion_tables) { + *mutable_grid_type() = GridType::PROBABILITY_GRID; +} ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { CHECK(proto.has_probability_grid_2d()); + *mutable_grid_type() = GridType::PROBABILITY_GRID; } // Sets the probability of the cell at 'cell_index' to the given diff --git a/cartographer/mapping/2d/probability_grid_test.cc b/cartographer/mapping/2d/probability_grid_test.cc index 99c3eb7..bf49156 100644 --- a/cartographer/mapping/2d/probability_grid_test.cc +++ b/cartographer/mapping/2d/probability_grid_test.cc @@ -44,11 +44,20 @@ TEST(ProbabilityGridTest, ProtoConstructor) { ValueConversionTables conversion_tables; ProbabilityGrid grid(proto, &conversion_tables); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); + EXPECT_EQ(grid.grid_type(), GridType::PROBABILITY_GRID); // TODO(macmason): Figure out how to test the contents of cells_ and // {min, max}_{x, y}_ gracefully. } +TEST(ProbabilityGridTest, ConstructorGridType) { + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), + &conversion_tables); + EXPECT_EQ(probability_grid.grid_type(), GridType::PROBABILITY_GRID); +} + TEST(ProbabilityGridTest, ToProto) { ValueConversionTables conversion_tables; ProbabilityGrid probability_grid( diff --git a/cartographer/mapping/2d/tsdf_2d.cc b/cartographer/mapping/2d/tsdf_2d.cc index c465ec7..f139ea9 100644 --- a/cartographer/mapping/2d/tsdf_2d.cc +++ b/cartographer/mapping/2d/tsdf_2d.cc @@ -30,12 +30,15 @@ TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance, truncation_distance, max_weight, conversion_tables_)), weight_cells_( limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells, - value_converter_->getUnknownWeightValue()) {} + value_converter_->getUnknownWeightValue()) { + *mutable_grid_type() = GridType::TSDF; +} TSDF2D::TSDF2D(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { CHECK(proto.has_tsdf_2d()); + *mutable_grid_type() = GridType::TSDF; value_converter_ = absl::make_unique( proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(), conversion_tables_); @@ -128,6 +131,7 @@ std::unique_ptr TSDF2D::ComputeCroppedGrid() const { cropped_grid->SetCell(xy_index, GetTSD(xy_index + offset), GetWeight(xy_index + offset)); } + cropped_grid->FinishUpdate(); return std::move(cropped_grid); } @@ -143,6 +147,7 @@ bool TSDF2D::DrawToSubmapTexture( if (!IsKnown(xy_index + offset)) { cells.push_back(0); // value cells.push_back(0); // alpha + continue; } // We would like to add 'delta' but this is not possible using a value and // alpha. We use premultiplied alpha, so when 'delta' is positive we can diff --git a/cartographer/mapping/2d/tsdf_2d_test.cc b/cartographer/mapping/2d/tsdf_2d_test.cc index 65f78e2..7182c82 100644 --- a/cartographer/mapping/2d/tsdf_2d_test.cc +++ b/cartographer/mapping/2d/tsdf_2d_test.cc @@ -48,9 +48,17 @@ TEST(TSDF2DTest, ProtoConstructor) { ValueConversionTables conversion_tables; TSDF2D grid(proto, &conversion_tables); + EXPECT_EQ(grid.grid_type(), GridType::TSDF); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); } +TEST(TSDF2DTest, ConstructorGridType) { + ValueConversionTables conversion_tables; + TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f, + 10.0f, &conversion_tables); + EXPECT_EQ(tsdf.grid_type(), GridType::TSDF); +} + TEST(TSDF2DTest, ToProto) { ValueConversionTables conversion_tables; TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,