12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/2d/grid_2d.cc
View File

@ -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)),

7
cartographer/mapping/2d/grid_2d.h
View File

@ -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_;

5
cartographer/mapping/2d/probability_grid.cc
View File

@ -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

9
cartographer/mapping/2d/probability_grid_test.cc
View File

@ -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(

7
cartographer/mapping/2d/tsdf_2d.cc
View File

@ -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

8
cartographer/mapping/2d/tsdf_2d_test.cc
View File

@ -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,