Inline a couple of 2D grid cell access functions (#1420)

Noticed ToFlatIndex during profiling. Inline a couple of other short functions as well.
cartographer-project · Oct 18, 2018 · 6ad7b87 · 6ad7b87
1 parent e1a839d
commit 6ad7b87
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 24 deletions.
diff --git 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
@@ -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_;