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

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

Noticed ToFlatIndex during profiling. Inline a couple of other short functions as well.
master
Juraj Oršulić 2018-10-18 15:21:56 +02:00 committed by gaschler
parent e1a839d668
commit 6ad7b87b67
2 changed files with 15 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -15,8 +15,6 @@
*/ */
#include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/probability_values.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace { 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 // Fills in 'offset' and 'limits' to define a subregion of that contains all
// known cells. // known cells.
void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset,
@ -198,10 +182,5 @@ proto::Grid2D Grid2D::ToProto() const {
return result; 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 mapping
} // namespace cartographer } // namespace cartographer

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

@ -21,6 +21,7 @@
#include "cartographer/mapping/2d/map_limits.h" #include "cartographer/mapping/2d/map_limits.h"
#include "cartographer/mapping/grid_interface.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/grid_2d.pb.h"
#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" #include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
@ -49,7 +50,11 @@ class Grid2D : public GridInterface {
void FinishUpdate(); void FinishUpdate();
// Returns the correspondence cost of the cell with 'cell_index'. // 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; virtual GridType GetGridType() const = 0;
@ -60,7 +65,11 @@ class Grid2D : public GridInterface {
float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; } float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; }
// Returns true if the probability at the specified index is known. // 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 // Fills in 'offset' and 'limits' to define a subregion of that contains all
// known cells. // known cells.
@ -101,7 +110,10 @@ class Grid2D : public GridInterface {
Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }
// Converts a 'cell_index' into an index into 'cells_'. // 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: private:
MapLimits limits_; MapLimits limits_;