Introduce GridType for Grid2D (#1355)

cartographer-project · Jul 31, 2018 · 6c070ac · 6c070ac
1 parent c1fbb6b
commit 6c070ac
Show file tree

Hide file tree

Showing 6 changed files with 36 additions and 2 deletions.
diff --git 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
@@ -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<uint16>* mutable_correspondence_cost_cells() {
     return &correspondence_cost_cells_;
   }
+
+  GridType* mutable_grid_type() { return &grid_type_; }
   std::vector<int>* 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<uint16> 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
@@ -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
@@ -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
@@ -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<TSDValueConverter>(
       proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(),
       conversion_tables_);
@@ -128,6 +131,7 @@ std::unique_ptr<Grid2D> 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
@@ -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,