diff --git a/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h b/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h index be209b6..f2b7b15 100644 --- a/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h +++ b/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h @@ -19,6 +19,7 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "ceres/ceres.h" diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index c6d7d19..c6e3ed9 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -23,6 +23,7 @@ #include "cartographer/io/draw_trajectories.h" #include "cartographer/io/image.h" #include "cartographer/io/points_batch.h" +#include "cartographer/mapping/probability_values.h" namespace cartographer { namespace io { diff --git a/cartographer/mapping_2d/probability_grid.cc b/cartographer/mapping_2d/probability_grid.cc new file mode 100644 index 0000000..8fd341e --- /dev/null +++ b/cartographer/mapping_2d/probability_grid.cc @@ -0,0 +1,164 @@ +#include "cartographer/mapping_2d/probability_grid.h" + +#include + +#include "cartographer/mapping/probability_values.h" + +namespace cartographer { +namespace mapping_2d { +namespace { + +// Converts a 'cell_index' into an index into 'cells_'. +int ToFlatIndex(const Eigen::Array2i& cell_index, const MapLimits& limits) { + CHECK(limits.Contains(cell_index)) << cell_index; + return limits.cell_limits().num_x_cells * cell_index.y() + cell_index.x(); +} + +} // namespace + +ProbabilityGrid::ProbabilityGrid(const MapLimits& limits) + : limits_(limits), + cells_( + limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, + mapping::kUnknownProbabilityValue) {} + +ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto) + : limits_(proto.limits()), cells_() { + if (proto.has_known_cells_box()) { + const auto& box = proto.known_cells_box(); + known_cells_box_ = + Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()), + Eigen::Vector2i(box.max_x(), box.max_y())); + } + cells_.reserve(proto.cells_size()); + for (const auto& cell : proto.cells()) { + CHECK_LE(cell, std::numeric_limits::max()); + cells_.push_back(cell); + } +} + +// Finishes the update sequence. +void ProbabilityGrid::FinishUpdate() { + while (!update_indices_.empty()) { + DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker); + cells_[update_indices_.back()] -= mapping::kUpdateMarker; + update_indices_.pop_back(); + } +} + +// Sets the probability of the cell at 'cell_index' to the given +// 'probability'. Only allowed if the cell was unknown before. +void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, + const float probability) { + uint16& cell = cells_[ToFlatIndex(cell_index, limits_)]; + CHECK_EQ(cell, mapping::kUnknownProbabilityValue); + cell = mapping::ProbabilityToValue(probability); + known_cells_box_.extend(cell_index.matrix()); +} + +// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() +// to the probability of the cell at 'cell_index' if the cell has not already +// been updated. Multiple updates of the same cell will be ignored until +// FinishUpdate() is called. Returns true if the cell was updated. +// +// If this is the first call to ApplyOdds() for the specified cell, its value +// will be set to probability corresponding to 'odds'. +bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, + const std::vector& table) { + DCHECK_EQ(table.size(), mapping::kUpdateMarker); + const int flat_index = ToFlatIndex(cell_index, limits_); + uint16* cell = &cells_[flat_index]; + if (*cell >= mapping::kUpdateMarker) { + return false; + } + update_indices_.push_back(flat_index); + *cell = table[*cell]; + DCHECK_GE(*cell, mapping::kUpdateMarker); + known_cells_box_.extend(cell_index.matrix()); + return true; +} + +// Returns the probability of the cell with 'cell_index'. +float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { + if (limits_.Contains(cell_index)) { + return mapping::ValueToProbability( + cells_[ToFlatIndex(cell_index, limits_)]); + } + return mapping::kMinProbability; +} + +// Returns true if the probability at the specified index is known. +bool ProbabilityGrid::IsKnown(const Eigen::Array2i& cell_index) const { + return limits_.Contains(cell_index) && + cells_[ToFlatIndex(cell_index, limits_)] != + mapping::kUnknownProbabilityValue; +} + +// Fills in 'offset' and 'limits' to define a subregion of that contains all +// known cells. +void ProbabilityGrid::ComputeCroppedLimits(Eigen::Array2i* const offset, + CellLimits* const limits) const { + if (known_cells_box_.isEmpty()) { + *offset = Eigen::Array2i::Zero(); + *limits = CellLimits(1, 1); + return; + } + *offset = known_cells_box_.min().array(); + *limits = CellLimits(known_cells_box_.sizes().x() + 1, + known_cells_box_.sizes().y() + 1); +} + +// Grows the map as necessary to include 'point'. This changes the meaning of +// these coordinates going forward. This method must be called immediately +// after 'FinishUpdate', before any calls to 'ApplyLookupTable'. +void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) { + CHECK(update_indices_.empty()); + while (!limits_.Contains(limits_.GetCellIndex(point))) { + const int x_offset = limits_.cell_limits().num_x_cells / 2; + const int y_offset = limits_.cell_limits().num_y_cells / 2; + const MapLimits new_limits( + limits_.resolution(), + limits_.max() + + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset), + CellLimits(2 * limits_.cell_limits().num_x_cells, + 2 * limits_.cell_limits().num_y_cells)); + const int stride = new_limits.cell_limits().num_x_cells; + const int offset = x_offset + stride * y_offset; + const int new_size = new_limits.cell_limits().num_x_cells * + new_limits.cell_limits().num_y_cells; + std::vector new_cells(new_size, mapping::kUnknownProbabilityValue); + for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { + for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { + new_cells[offset + j + i * stride] = + cells_[j + i * limits_.cell_limits().num_x_cells]; + } + } + cells_ = new_cells; + limits_ = new_limits; + if (!known_cells_box_.isEmpty()) { + known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset)); + } + } +} + +proto::ProbabilityGrid ProbabilityGrid::ToProto() const { + proto::ProbabilityGrid result; + *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_); + result.mutable_cells()->Reserve(cells_.size()); + for (const auto& cell : cells_) { + result.mutable_cells()->Add(cell); + } + CHECK(update_indices_.empty()) << "Serializing a grid during an update is " + "not supported. Finish the update first."; + if (!known_cells_box_.isEmpty()) { + auto* const box = result.mutable_known_cells_box(); + box->set_max_x(known_cells_box_.max().x()); + box->set_max_y(known_cells_box_.max().y()); + box->set_min_x(known_cells_box_.min().x()); + box->set_min_y(known_cells_box_.min().y()); + } + return result; +} + +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index fccc6bb..1963ec7 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -17,21 +17,12 @@ #ifndef CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ #define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ -#include -#include -#include -#include -#include -#include #include -#include "cartographer/common/math.h" #include "cartographer/common/port.h" -#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/proto/probability_grid.pb.h" #include "cartographer/mapping_2d/xy_index.h" -#include "glog/logging.h" namespace cartographer { namespace mapping_2d { @@ -39,48 +30,19 @@ namespace mapping_2d { // Represents a 2D grid of probabilities. class ProbabilityGrid { public: - explicit ProbabilityGrid(const MapLimits& limits) - : limits_(limits), - cells_(limits_.cell_limits().num_x_cells * - limits_.cell_limits().num_y_cells, - mapping::kUnknownProbabilityValue) {} - - explicit ProbabilityGrid(const proto::ProbabilityGrid& proto) - : limits_(proto.limits()), cells_() { - if (proto.has_known_cells_box()) { - const auto& box = proto.known_cells_box(); - known_cells_box_ = - Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()), - Eigen::Vector2i(box.max_x(), box.max_y())); - } - cells_.reserve(proto.cells_size()); - for (const auto cell : proto.cells()) { - CHECK_LE(cell, std::numeric_limits::max()); - cells_.push_back(cell); - } - } + explicit ProbabilityGrid(const MapLimits& limits); + explicit ProbabilityGrid(const proto::ProbabilityGrid& proto); // Returns the limits of this ProbabilityGrid. const MapLimits& limits() const { return limits_; } // Finishes the update sequence. - void FinishUpdate() { - while (!update_indices_.empty()) { - DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker); - cells_[update_indices_.back()] -= mapping::kUpdateMarker; - update_indices_.pop_back(); - } - } + void FinishUpdate(); // Sets the probability of the cell at 'cell_index' to the given // 'probability'. Only allowed if the cell was unknown before. void SetProbability(const Eigen::Array2i& cell_index, - const float probability) { - uint16& cell = cells_[ToFlatIndex(cell_index)]; - CHECK_EQ(cell, mapping::kUnknownProbabilityValue); - cell = mapping::ProbabilityToValue(probability); - known_cells_box_.extend(cell_index.matrix()); - } + const float probability); // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() // to the probability of the cell at 'cell_index' if the cell has not already @@ -90,108 +52,27 @@ class ProbabilityGrid { // If this is the first call to ApplyOdds() for the specified cell, its value // will be set to probability corresponding to 'odds'. bool ApplyLookupTable(const Eigen::Array2i& cell_index, - const std::vector& table) { - DCHECK_EQ(table.size(), mapping::kUpdateMarker); - const int flat_index = ToFlatIndex(cell_index); - uint16& cell = cells_[flat_index]; - if (cell >= mapping::kUpdateMarker) { - return false; - } - update_indices_.push_back(flat_index); - cell = table[cell]; - DCHECK_GE(cell, mapping::kUpdateMarker); - known_cells_box_.extend(cell_index.matrix()); - return true; - } + const std::vector& table); // Returns the probability of the cell with 'cell_index'. - float GetProbability(const Eigen::Array2i& cell_index) const { - if (limits_.Contains(cell_index)) { - return mapping::ValueToProbability(cells_[ToFlatIndex(cell_index)]); - } - return mapping::kMinProbability; - } + float GetProbability(const Eigen::Array2i& cell_index) const; // Returns true if the probability at the specified index is known. - bool IsKnown(const Eigen::Array2i& cell_index) const { - return limits_.Contains(cell_index) && - cells_[ToFlatIndex(cell_index)] != mapping::kUnknownProbabilityValue; - } + bool IsKnown(const Eigen::Array2i& cell_index) const; // Fills in 'offset' and 'limits' to define a subregion of that contains all // known cells. void ComputeCroppedLimits(Eigen::Array2i* const offset, - CellLimits* const limits) const { - if (known_cells_box_.isEmpty()) { - *offset = Eigen::Array2i::Zero(); - *limits = CellLimits(1, 1); - } else { - *offset = known_cells_box_.min().array(); - *limits = CellLimits(known_cells_box_.sizes().x() + 1, - known_cells_box_.sizes().y() + 1); - } - } + CellLimits* const limits) const; // Grows the map as necessary to include 'point'. This changes the meaning of // these coordinates going forward. This method must be called immediately // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. - void GrowLimits(const Eigen::Vector2f& point) { - CHECK(update_indices_.empty()); - while (!limits_.Contains(limits_.GetCellIndex(point))) { - const int x_offset = limits_.cell_limits().num_x_cells / 2; - const int y_offset = limits_.cell_limits().num_y_cells / 2; - const MapLimits new_limits( - limits_.resolution(), - limits_.max() + - limits_.resolution() * Eigen::Vector2d(y_offset, x_offset), - CellLimits(2 * limits_.cell_limits().num_x_cells, - 2 * limits_.cell_limits().num_y_cells)); - const int stride = new_limits.cell_limits().num_x_cells; - const int offset = x_offset + stride * y_offset; - const int new_size = new_limits.cell_limits().num_x_cells * - new_limits.cell_limits().num_y_cells; - std::vector new_cells(new_size, - mapping::kUnknownProbabilityValue); - for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { - for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { - new_cells[offset + j + i * stride] = - cells_[j + i * limits_.cell_limits().num_x_cells]; - } - } - cells_ = new_cells; - limits_ = new_limits; - if (!known_cells_box_.isEmpty()) { - known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset)); - } - } - } + void GrowLimits(const Eigen::Vector2f& point); - proto::ProbabilityGrid ToProto() const { - proto::ProbabilityGrid result; - *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_); - result.mutable_cells()->Reserve(cells_.size()); - for (const auto cell : cells_) { - result.mutable_cells()->Add(cell); - } - CHECK(update_indices_.empty()) << "Serializing a grid during an update is " - "not supported. Finish the update first."; - if (!known_cells_box_.isEmpty()) { - auto* const box = result.mutable_known_cells_box(); - box->set_max_x(known_cells_box_.max().x()); - box->set_max_y(known_cells_box_.max().y()); - box->set_min_x(known_cells_box_.min().x()); - box->set_min_y(known_cells_box_.min().y()); - } - return result; - } + proto::ProbabilityGrid ToProto() const; private: - // Converts a 'cell_index' into an index into 'cells_'. - int ToFlatIndex(const Eigen::Array2i& cell_index) const { - CHECK(limits_.Contains(cell_index)) << cell_index; - return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x(); - } - MapLimits limits_; std::vector cells_; // Highest bit is update marker. std::vector update_indices_; diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index ddd366c..c8a1a75 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -17,14 +17,17 @@ #include "cartographer/mapping_2d/probability_grid.h" #include -#include +#include "cartographer/mapping/probability_values.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace { +using Eigen::Array2i; +using Eigen::Vector2f; + TEST(ProbabilityGridTest, ProtoConstructor) { proto::ProbabilityGrid proto; const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.)); @@ -61,49 +64,47 @@ TEST(ProbabilityGridTest, ApplyOdds) { MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2))); const MapLimits& limits = probability_grid.limits(); - EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0))); - EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 1))); - EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 0))); - EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 1))); - EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 0))); - EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 1))); - EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 0))); - EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 1))); + EXPECT_TRUE(limits.Contains(Array2i(0, 0))); + EXPECT_TRUE(limits.Contains(Array2i(0, 1))); + EXPECT_TRUE(limits.Contains(Array2i(1, 0))); + EXPECT_TRUE(limits.Contains(Array2i(1, 1))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 0))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 1))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 0))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 1))); - probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5); + probability_grid.SetProbability(Array2i(1, 0), 0.5); probability_grid.ApplyLookupTable( - Eigen::Array2i(1, 0), + Array2i(1, 0), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); probability_grid.FinishUpdate(); - EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5); + EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5); - probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5); + probability_grid.SetProbability(Array2i(0, 1), 0.5); probability_grid.ApplyLookupTable( - Eigen::Array2i(0, 1), + Array2i(0, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1))); probability_grid.FinishUpdate(); - EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5); + EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5); // Tests adding odds to an unknown cell. probability_grid.ApplyLookupTable( - Eigen::Array2i(1, 1), + Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42))); - EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42, - 1e-4); + EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4); // Tests that further updates are ignored if FinishUpdate() isn't called. probability_grid.ApplyLookupTable( - Eigen::Array2i(1, 1), + Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); - EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42, - 1e-4); + EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4); probability_grid.FinishUpdate(); probability_grid.ApplyLookupTable( - Eigen::Array2i(1, 1), + Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); - EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42); + EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42); } TEST(ProbabilityGridTest, GetProbability) { @@ -118,16 +119,14 @@ TEST(ProbabilityGridTest, GetProbability) { ASSERT_EQ(2, cell_limits.num_x_cells); ASSERT_EQ(2, cell_limits.num_y_cells); - probability_grid.SetProbability( - limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)), - mapping::kMaxProbability); + probability_grid.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)), + mapping::kMaxProbability); EXPECT_NEAR(probability_grid.GetProbability( - limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f))), + limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), mapping::kMaxProbability, 1e-6); - for (const Eigen::Array2i& xy_index : - {limits.GetCellIndex(Eigen::Vector2f(-0.5f, 1.5f)), - limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f)), - limits.GetCellIndex(Eigen::Vector2f(0.5f, 1.5f))}) { + for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)), + limits.GetCellIndex(Vector2f(0.5f, 0.5f)), + limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) { EXPECT_TRUE(limits.Contains(xy_index)); EXPECT_FALSE(probability_grid.IsKnown(xy_index)); } @@ -142,34 +141,25 @@ TEST(ProbabilityGridTest, GetCellIndex) { ASSERT_EQ(14, cell_limits.num_x_cells); ASSERT_EQ(8, cell_limits.num_y_cells); EXPECT_TRUE( - (Eigen::Array2i(0, 0) == limits.GetCellIndex(Eigen::Vector2f(7.f, 13.f))) - .all()); - EXPECT_TRUE((Eigen::Array2i(13, 0) == - limits.GetCellIndex(Eigen::Vector2f(7.f, -13.f))) - .all()); + (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all()); EXPECT_TRUE( - (Eigen::Array2i(0, 7) == limits.GetCellIndex(Eigen::Vector2f(-7.f, 13.f))) - .all()); - EXPECT_TRUE((Eigen::Array2i(13, 7) == - limits.GetCellIndex(Eigen::Vector2f(-7.f, -13.f))) - .all()); + (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all()); + EXPECT_TRUE( + (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all()); + EXPECT_TRUE( + (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all()); // Check around the origin. EXPECT_TRUE( - (Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f))) - .all()); + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all()); EXPECT_TRUE( - (Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(1.5f, 1.5f))) - .all()); - EXPECT_TRUE((Eigen::Array2i(7, 3) == - limits.GetCellIndex(Eigen::Vector2f(0.5f, -0.5f))) - .all()); - EXPECT_TRUE((Eigen::Array2i(6, 4) == - limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f))) - .all()); - EXPECT_TRUE((Eigen::Array2i(7, 4) == - limits.GetCellIndex(Eigen::Vector2f(-0.5f, -0.5f))) - .all()); + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all()); + EXPECT_TRUE( + (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all()); } TEST(ProbabilityGridTest, CorrectCropping) { @@ -179,14 +169,14 @@ TEST(ProbabilityGridTest, CorrectCropping) { mapping::kMinProbability, mapping::kMaxProbability); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400))); - for (const Eigen::Array2i& xy_index : XYIndexRangeIterator( - Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) { + for (const Array2i& xy_index : + XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) { probability_grid.SetProbability(xy_index, value_distribution(rng)); } - Eigen::Array2i offset; + Array2i offset; CellLimits limits; probability_grid.ComputeCroppedLimits(&offset, &limits); - EXPECT_TRUE((offset == Eigen::Array2i(100, 100)).all()); + EXPECT_TRUE((offset == Array2i(100, 100)).all()); EXPECT_EQ(limits.num_x_cells, 200); EXPECT_EQ(limits.num_y_cells, 200); } diff --git a/cartographer/mapping_2d/range_data_inserter.cc b/cartographer/mapping_2d/range_data_inserter.cc index ba13ed4..24b5486 100644 --- a/cartographer/mapping_2d/range_data_inserter.cc +++ b/cartographer/mapping_2d/range_data_inserter.cc @@ -20,6 +20,7 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/ray_casting.h" #include "cartographer/mapping_2d/xy_index.h" #include "glog/logging.h" diff --git a/cartographer/mapping_2d/range_data_inserter_test.cc b/cartographer/mapping_2d/range_data_inserter_test.cc index 9c76a7a..33c6303 100644 --- a/cartographer/mapping_2d/range_data_inserter_test.cc +++ b/cartographer/mapping_2d/range_data_inserter_test.cc @@ -21,6 +21,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" +#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/probability_grid.h" #include "gmock/gmock.h" diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc index 39e36d0..f21a9ed 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -21,6 +21,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" +#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform_test_helpers.h" diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h index 0218233..e0f343e 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h @@ -30,6 +30,7 @@ #include "Eigen/Core" #include "cartographer/common/port.h" +#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"