diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc index d64d897..c2fe23e 100644 --- a/cartographer/mapping/2d/grid_2d.cc +++ b/cartographer/mapping/2d/grid_2d.cc @@ -15,8 +15,6 @@ */ #include "cartographer/mapping/2d/grid_2d.h" -#include "cartographer/mapping/probability_values.h" - namespace cartographer { namespace mapping { namespace { @@ -107,20 +105,6 @@ void Grid2D::FinishUpdate() { } } -// Returns the correspondence cost of the cell with 'cell_index'. -float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const { - if (!limits().Contains(cell_index)) return max_correspondence_cost_; - return (*value_to_correspondence_cost_table_) - [correspondence_cost_cells()[ToFlatIndex(cell_index)]]; -} - -// Returns true if the correspondence cost at the specified index is known. -bool Grid2D::IsKnown(const Eigen::Array2i& cell_index) const { - return limits_.Contains(cell_index) && - correspondence_cost_cells_[ToFlatIndex(cell_index)] != - kUnknownCorrespondenceValue; -} - // Fills in 'offset' and 'limits' to define a subregion of that contains all // known cells. void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, @@ -198,10 +182,5 @@ proto::Grid2D Grid2D::ToProto() const { return result; } -int Grid2D::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(); -} - } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index c62f55a..527c456 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -21,6 +21,7 @@ #include "cartographer/mapping/2d/map_limits.h" #include "cartographer/mapping/grid_interface.h" +#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/proto/2d/grid_2d.pb.h" #include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" @@ -49,7 +50,11 @@ class Grid2D : public GridInterface { void FinishUpdate(); // Returns the correspondence cost of the cell with 'cell_index'. - float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const; + float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const { + if (!limits().Contains(cell_index)) return max_correspondence_cost_; + return (*value_to_correspondence_cost_table_) + [correspondence_cost_cells()[ToFlatIndex(cell_index)]]; + } virtual GridType GetGridType() const = 0; @@ -60,7 +65,11 @@ class Grid2D : public GridInterface { float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; } // Returns true if the probability at the specified index is known. - bool IsKnown(const Eigen::Array2i& cell_index) const; + bool IsKnown(const Eigen::Array2i& cell_index) const { + return limits_.Contains(cell_index) && + correspondence_cost_cells_[ToFlatIndex(cell_index)] != + kUnknownCorrespondenceValue; + } // Fills in 'offset' and 'limits' to define a subregion of that contains all // known cells. @@ -101,7 +110,10 @@ class Grid2D : public GridInterface { Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } // Converts a 'cell_index' into an index into 'cells_'. - int ToFlatIndex(const Eigen::Array2i& cell_index) const; + 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(); + } private: MapLimits limits_;