Introduce GridType for Grid2D (#1355)

master
Kevin Daun 2018-07-31 15:13:37 +02:00 committed by Wally B. Feed
parent c1fbb6bb07
commit 6c070acff5
6 changed files with 36 additions and 2 deletions

View File

@ -63,6 +63,7 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
float max_correspondence_cost, float max_correspondence_cost,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: limits_(limits), : limits_(limits),
grid_type_(GridType::NONE),
correspondence_cost_cells_( correspondence_cost_cells_(
limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
kUnknownCorrespondenceValue), kUnknownCorrespondenceValue),
@ -77,6 +78,7 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
Grid2D::Grid2D(const proto::Grid2D& proto, Grid2D::Grid2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: limits_(proto.limits()), : limits_(proto.limits()),
grid_type_(GridType::NONE),
correspondence_cost_cells_(), correspondence_cost_cells_(),
min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)), min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)),
max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)), max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)),

View File

@ -32,6 +32,8 @@ namespace mapping {
proto::GridOptions2D CreateGridOptions2D( proto::GridOptions2D CreateGridOptions2D(
common::LuaParameterDictionary* const parameter_dictionary); common::LuaParameterDictionary* const parameter_dictionary);
enum class GridType { NONE, PROBABILITY_GRID, TSDF };
class Grid2D : public GridInterface { class Grid2D : public GridInterface {
public: public:
Grid2D(const MapLimits& limits, float min_correspondence_cost, Grid2D(const MapLimits& limits, float min_correspondence_cost,
@ -43,6 +45,8 @@ class Grid2D : public GridInterface {
// Returns the limits of this Grid2D. // Returns the limits of this Grid2D.
const MapLimits& limits() const { return limits_; } const MapLimits& limits() const { return limits_; }
GridType grid_type() const { return grid_type_; }
// Finishes the update sequence. // Finishes the update sequence.
void FinishUpdate(); void FinishUpdate();
// Returns the correspondence cost of the cell with 'cell_index'. // Returns the correspondence cost of the cell with 'cell_index'.
@ -91,6 +95,8 @@ class Grid2D : public GridInterface {
std::vector<uint16>* mutable_correspondence_cost_cells() { std::vector<uint16>* mutable_correspondence_cost_cells() {
return &correspondence_cost_cells_; return &correspondence_cost_cells_;
} }
GridType* mutable_grid_type() { return &grid_type_; }
std::vector<int>* mutable_update_indices() { return &update_indices_; } std::vector<int>* mutable_update_indices() { return &update_indices_; }
Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }
@ -99,6 +105,7 @@ class Grid2D : public GridInterface {
private: private:
MapLimits limits_; MapLimits limits_;
GridType grid_type_;
std::vector<uint16> correspondence_cost_cells_; std::vector<uint16> correspondence_cost_cells_;
float min_correspondence_cost_; float min_correspondence_cost_;
float max_correspondence_cost_; float max_correspondence_cost_;

View File

@ -28,12 +28,15 @@ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost, : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
conversion_tables), conversion_tables),
conversion_tables_(conversion_tables) {} conversion_tables_(conversion_tables) {
*mutable_grid_type() = GridType::PROBABILITY_GRID;
}
ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto, ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
CHECK(proto.has_probability_grid_2d()); CHECK(proto.has_probability_grid_2d());
*mutable_grid_type() = GridType::PROBABILITY_GRID;
} }
// Sets the probability of the cell at 'cell_index' to the given // Sets the probability of the cell at 'cell_index' to the given

View File

@ -44,11 +44,20 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
ProbabilityGrid grid(proto, &conversion_tables); ProbabilityGrid grid(proto, &conversion_tables);
EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); 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 // TODO(macmason): Figure out how to test the contents of cells_ and
// {min, max}_{x, y}_ gracefully. // {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) { TEST(ProbabilityGridTest, ToProto) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid( ProbabilityGrid probability_grid(

View File

@ -30,12 +30,15 @@ TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance,
truncation_distance, max_weight, conversion_tables_)), truncation_distance, max_weight, conversion_tables_)),
weight_cells_( weight_cells_(
limits.cell_limits().num_x_cells * limits.cell_limits().num_y_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, TSDF2D::TSDF2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
CHECK(proto.has_tsdf_2d()); CHECK(proto.has_tsdf_2d());
*mutable_grid_type() = GridType::TSDF;
value_converter_ = absl::make_unique<TSDValueConverter>( value_converter_ = absl::make_unique<TSDValueConverter>(
proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(), proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(),
conversion_tables_); conversion_tables_);
@ -128,6 +131,7 @@ std::unique_ptr<Grid2D> TSDF2D::ComputeCroppedGrid() const {
cropped_grid->SetCell(xy_index, GetTSD(xy_index + offset), cropped_grid->SetCell(xy_index, GetTSD(xy_index + offset),
GetWeight(xy_index + offset)); GetWeight(xy_index + offset));
} }
cropped_grid->FinishUpdate();
return std::move(cropped_grid); return std::move(cropped_grid);
} }
@ -143,6 +147,7 @@ bool TSDF2D::DrawToSubmapTexture(
if (!IsKnown(xy_index + offset)) { if (!IsKnown(xy_index + offset)) {
cells.push_back(0); // value cells.push_back(0); // value
cells.push_back(0); // alpha cells.push_back(0); // alpha
continue;
} }
// We would like to add 'delta' but this is not possible using a value and // 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 // alpha. We use premultiplied alpha, so when 'delta' is positive we can

View File

@ -48,9 +48,17 @@ TEST(TSDF2DTest, ProtoConstructor) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
TSDF2D grid(proto, &conversion_tables); TSDF2D grid(proto, &conversion_tables);
EXPECT_EQ(grid.grid_type(), GridType::TSDF);
EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); 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) { TEST(TSDF2DTest, ToProto) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f, TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,