From 5332cc278387182684dbd68a4c60e996a7b93400 Mon Sep 17 00:00:00 2001 From: Vaibhav Viswanathan Date: Wed, 21 Oct 2020 12:59:43 -0400 Subject: [PATCH] Added GetPointFromCellIndex in map_limits.h --- cartographer/mapping/2d/map_limits.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/cartographer/mapping/2d/map_limits.h b/cartographer/mapping/2d/map_limits.h index 241fc570a7..7eceb0e45b 100644 --- a/cartographer/mapping/2d/map_limits.h +++ b/cartographer/mapping/2d/map_limits.h @@ -75,6 +75,17 @@ class MapLimits { common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5)); } + // Returns the "point" given the index of the cell + // inverse of GetCellIndex + Eigen::Vector2f GetPointFromCellIndex(const Eigen::Array2i& cellIndex) const { + // Index values are row major and the top left has Eigen::Array2i::Zero() + // and contains (centered_max_x, centered_max_y). We need to flip and + // rotate. + return Eigen::Vector2f( + max_.y() - resolution_ * ((float)cellIndex.x() + 0.5), + max_.x() - resolution_ * ((float)cellIndex.y() + 0.5)); + } + // Returns the center of the cell at 'cell_index'. Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const { return {max_.x() - resolution() * (cell_index[1] + 0.5),