diff --git a/cartographer/mapping/submaps.cc b/cartographer/mapping/submaps.cc index eb65250..4bce183 100644 --- a/cartographer/mapping/submaps.cc +++ b/cartographer/mapping/submaps.cc @@ -80,10 +80,10 @@ void Submaps::AddProbabilityGridToResponse( response->set_height(limits.num_y_cells); const double resolution = probability_grid.limits().resolution(); response->set_resolution(resolution); - const double max_x = probability_grid.limits().edge_limits().max().x() - - resolution * offset.y(); - const double max_y = probability_grid.limits().edge_limits().max().y() - - resolution * offset.x(); + const double max_x = + probability_grid.limits().max().x() - resolution * offset.y(); + const double max_y = + probability_grid.limits().max().y() - resolution * offset.x(); *response->mutable_slice_pose() = transform::ToProto( local_submap_pose.inverse() * transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); diff --git a/cartographer/mapping_2d/CMakeLists.txt b/cartographer/mapping_2d/CMakeLists.txt index e12a53e..d5db781 100644 --- a/cartographer/mapping_2d/CMakeLists.txt +++ b/cartographer/mapping_2d/CMakeLists.txt @@ -144,6 +144,7 @@ google_library(mapping_2d_submaps DEPENDS common_lua_parameter_dictionary common_make_unique + common_port mapping_2d_laser_fan_inserter mapping_2d_map_limits mapping_2d_probability_grid diff --git a/cartographer/mapping_2d/laser_fan_inserter_test.cc b/cartographer/mapping_2d/laser_fan_inserter_test.cc index f7be710..d9c6beb 100644 --- a/cartographer/mapping_2d/laser_fan_inserter_test.cc +++ b/cartographer/mapping_2d/laser_fan_inserter_test.cc @@ -32,12 +32,7 @@ class LaserFanInserterTest : public ::testing::Test { protected: LaserFanInserterTest() : probability_grid_( - MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.), - Eigen::Vector2d(0., 4.)))) { - const CellLimits& cell_limits = probability_grid_.limits().cell_limits(); - CHECK_EQ(5, cell_limits.num_x_cells); - CHECK_EQ(5, cell_limits.num_y_cells); - + MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) { auto parameter_dictionary = common::MakeDictionary( "return { " "insert_free_space = true, " @@ -68,12 +63,8 @@ class LaserFanInserterTest : public ::testing::Test { TEST_F(LaserFanInserterTest, InsertPointCloud) { InsertPointCloud(); - const Eigen::AlignedBox2d& edge_limits = - probability_grid_.limits().edge_limits(); - EXPECT_NEAR(-4., edge_limits.min().x(), 1e-9); - EXPECT_NEAR(0., edge_limits.min().y(), 1e-9); - EXPECT_NEAR(1., edge_limits.max().x(), 1e-9); - EXPECT_NEAR(5., edge_limits.max().y(), 1e-9); + EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9); + EXPECT_NEAR(5., probability_grid_.limits().max().y(), 1e-9); const CellLimits& cell_limits = probability_grid_.limits().cell_limits(); EXPECT_EQ(5, cell_limits.num_x_cells); diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index 49173e1..b6119dd 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -38,29 +38,21 @@ namespace mapping_2d { // performance reasons. class MapLimits { public: - MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits) - : resolution_(resolution) { - SetLimits(edge_limits); - } - - MapLimits(const double resolution, const double max_x, const double max_y, + MapLimits(const double resolution, const Eigen::Vector2d& max, const CellLimits& cell_limits) - : resolution_(resolution) { - SetLimits(max_x, max_y, cell_limits); + : resolution_(resolution), max_(max), cell_limits_(cell_limits) { + CHECK_GT(resolution_, 0.); + CHECK_GT(cell_limits.num_x_cells, 0.); + CHECK_GT(cell_limits.num_y_cells, 0.); } // Returns the cell size in meters. All cells are square and the resolution is // the length of one side. double resolution() const { return resolution_; } - // Returns the limits of the grid from edge to edge in meters. - const Eigen::AlignedBox2d& edge_limits() const { return edge_limits_; } - - // Returns the limits of the grid between the centers of the edge cells in - // meters. - const Eigen::AlignedBox2d& centered_limits() const { - return centered_limits_; - } + // Returns the corner of the limits, i.e., all pixels have positions with + // smaller coordinates. + const Eigen::Vector2d& max() const { return max_; } // Returns the limits of the grid in number of cells. const CellLimits& cell_limits() const { return cell_limits_; } @@ -74,8 +66,8 @@ class MapLimits { // and contains (centered_max_x, centered_max_y). We need to flip and // rotate. return Eigen::Array2i( - common::RoundToInt((centered_limits_.max().y() - y) / resolution_), - common::RoundToInt((centered_limits_.max().x() - x) / resolution_)); + common::RoundToInt((max_.y() - y) / resolution_ - 0.5), + common::RoundToInt((max_.x() - x) / resolution_ - 0.5)); } // Returns true of the ProbabilityGrid contains 'xy_index'. @@ -97,7 +89,9 @@ class MapLimits { const float kPadding = 3.f * resolution; bounding_box.min() -= kPadding * Eigen::Vector2f::Ones(); bounding_box.max() += kPadding * Eigen::Vector2f::Ones(); - return MapLimits(resolution, bounding_box.cast()); + return MapLimits(resolution, bounding_box.max().cast(), + CellLimits(common::RoundToInt(bounding_box.sizes().y()), + common::RoundToInt(bounding_box.sizes().x()))); } static Eigen::AlignedBox2f ComputeMapBoundingBox( @@ -128,49 +122,8 @@ class MapLimits { } private: - // Returns the center of the resolution interval containing 'x'. - double Center(const double x) { - return (std::floor(x / resolution_) + 0.5) * resolution_; - } - - // Sets the limits of the grid to the specified bounding box in meters. - void SetLimits(const Eigen::AlignedBox2d& limits) { - CHECK(!limits.isEmpty()); - const int num_x_cells = common::RoundToInt((Center(limits.max().y()) - - Center(limits.min().y())) / - resolution_) + - 1; - const int num_y_cells = common::RoundToInt((Center(limits.max().x()) - - Center(limits.min().x())) / - resolution_) + - 1; - SetLimits(limits.max().x(), limits.max().y(), - CellLimits(num_x_cells, num_y_cells)); - } - - void SetLimits(double max_x, double max_y, const CellLimits& limits) { - CHECK_GT(resolution_, 0.); - CHECK_GT(limits.num_x_cells, 0.); - CHECK_GT(limits.num_y_cells, 0.); - - cell_limits_ = limits; - centered_limits_.max().x() = Center(max_x); - centered_limits_.max().y() = Center(max_y); - centered_limits_.min().x() = centered_limits_.max().x() - - resolution_ * (cell_limits_.num_y_cells - 1); - centered_limits_.min().y() = centered_limits_.max().y() - - resolution_ * (cell_limits_.num_x_cells - 1); - - const double half_resolution = resolution_ / 2.; - edge_limits_.min().x() = centered_limits_.min().x() - half_resolution; - edge_limits_.min().y() = centered_limits_.min().y() - half_resolution; - edge_limits_.max().x() = centered_limits_.max().x() + half_resolution; - edge_limits_.max().y() = centered_limits_.max().y() + half_resolution; - } - double resolution_; - Eigen::AlignedBox2d edge_limits_; - Eigen::AlignedBox2d centered_limits_; + Eigen::Vector2d max_; CellLimits cell_limits_; }; diff --git a/cartographer/mapping_2d/map_limits_test.cc b/cartographer/mapping_2d/map_limits_test.cc index 29d7e11..fb8d5d3 100644 --- a/cartographer/mapping_2d/map_limits_test.cc +++ b/cartographer/mapping_2d/map_limits_test.cc @@ -22,43 +22,18 @@ namespace cartographer { namespace mapping_2d { namespace { -TEST(MapLimits, SetEdgeLimits) { - const MapLimits limits(2., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 1.5), - Eigen::Vector2d(0.5, 5.5))); - - const Eigen::AlignedBox2d& edge_limits = limits.edge_limits(); - EXPECT_EQ(-2., edge_limits.min().x()); - EXPECT_EQ(0., edge_limits.min().y()); - EXPECT_EQ(2., edge_limits.max().x()); - EXPECT_EQ(6., edge_limits.max().y()); - - const CellLimits& cell_limits = limits.cell_limits(); - EXPECT_EQ(3, cell_limits.num_x_cells); - EXPECT_EQ(2, cell_limits.num_y_cells); -} - -TEST(MapLimits, SetCellLimits) { - const MapLimits limits(3., 1.5, -0.5, CellLimits(2, 3)); +TEST(MapLimits, ConstructAndGet) { + const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3)); const CellLimits& cell_limits = limits.cell_limits(); EXPECT_EQ(2, cell_limits.num_x_cells); EXPECT_EQ(3, cell_limits.num_y_cells); - const Eigen::AlignedBox2d& edge_limits = limits.edge_limits(); - EXPECT_EQ(-6., edge_limits.min().x()); - EXPECT_EQ(-6., edge_limits.min().y()); - EXPECT_EQ(3., edge_limits.max().x()); - EXPECT_EQ(0., edge_limits.max().y()); -} + const Eigen::Vector2d& max = limits.max(); + EXPECT_EQ(3., max.x()); + EXPECT_EQ(0., max.y()); -TEST(MapLimits, GetCenteredLimits) { - const MapLimits limits(2., -0.5, 1.5, CellLimits(3, 2)); - - const Eigen::AlignedBox2d& centered_limits = limits.centered_limits(); - EXPECT_EQ(-3, centered_limits.min().x()); - EXPECT_EQ(-3., centered_limits.min().y()); - EXPECT_EQ(-1., centered_limits.max().x()); - EXPECT_EQ(1., centered_limits.max().y()); + EXPECT_EQ(42., limits.resolution()); } } // namespace diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index ea3e6eb..c6ebdaf 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -130,8 +130,8 @@ class ProbabilityGrid { const int y_offset = limits_.cell_limits().num_y_cells / 2; const MapLimits new_limits( limits_.resolution(), - limits_.centered_limits().max().x() + y_offset * limits_.resolution(), - limits_.centered_limits().max().y() + x_offset * 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; diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index d41e615..cb16685 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -25,7 +25,8 @@ namespace mapping_2d { namespace { TEST(ProbabilityGridTest, ApplyOdds) { - ProbabilityGrid probability_grid(MapLimits(1., 0.5, 0.5, CellLimits(2, 2))); + ProbabilityGrid probability_grid( + MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2))); const MapLimits& limits = probability_grid.limits(); EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0))); @@ -77,15 +78,11 @@ TEST(ProbabilityGridTest, ApplyOdds) { TEST(ProbabilityGridTest, GetProbability) { ProbabilityGrid probability_grid( - MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 0.5), - Eigen::Vector2d(0.5, 1.5)))); + MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2))); const MapLimits& limits = probability_grid.limits(); - const Eigen::AlignedBox2d& edge_limits = limits.edge_limits(); - EXPECT_EQ(-1., edge_limits.min().x()); - EXPECT_EQ(0., edge_limits.min().y()); - EXPECT_EQ(1., edge_limits.max().x()); - EXPECT_EQ(2., edge_limits.max().y()); + EXPECT_EQ(1., limits.max().x()); + EXPECT_EQ(2., limits.max().y()); const CellLimits& cell_limits = limits.cell_limits(); ASSERT_EQ(2, cell_limits.num_x_cells); @@ -107,7 +104,8 @@ TEST(ProbabilityGridTest, GetProbability) { } TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { - ProbabilityGrid probability_grid(MapLimits(2., 7, 13, CellLimits(14, 8))); + ProbabilityGrid probability_grid( + MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8))); const MapLimits& limits = probability_grid.limits(); const CellLimits& cell_limits = limits.cell_limits(); @@ -150,7 +148,7 @@ TEST(ProbabilityGridTest, CorrectCropping) { std::uniform_real_distribution value_distribution( mapping::kMinProbability, mapping::kMaxProbability); ProbabilityGrid probability_grid( - MapLimits(0.05, 10., 10., CellLimits(400, 400))); + MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator( Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) { diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index 04b4451..16231fe 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/cartographer/mapping_2d/ray_casting.cc @@ -151,12 +151,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits, const std::function& hit_visitor, const std::function& miss_visitor) { const double superscaled_resolution = limits.resolution() / kSubpixelScale; - const Eigen::Vector2d superscaled_centered_max = - limits.edge_limits().max() - - superscaled_resolution / 2. * Eigen::Vector2d::Ones(); const MapLimits superscaled_limits( - superscaled_resolution, superscaled_centered_max.x(), - superscaled_centered_max.y(), + superscaled_resolution, limits.max(), CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, limits.cell_limits().num_y_cells * kSubpixelScale)); const Eigen::Array2i begin = 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 a333127..2667dd0 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -35,8 +35,7 @@ class CeresScanMatcherTest : public ::testing::Test { protected: CeresScanMatcherTest() : probability_grid_( - MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-10., -10.), - Eigen::Vector2d(10., 10.)))) { + MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) { probability_grid_.SetProbability( probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5), mapping::kMaxProbability); diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc index 231ee7a..a039045 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc @@ -79,9 +79,8 @@ TEST(DiscretizeScans, DiscretizeScans) { point_cloud.emplace_back(-0.125, 0.125); point_cloud.emplace_back(-0.125, 0.075); point_cloud.emplace_back(-0.125, 0.025); - const MapLimits map_limits( - 0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025), - Eigen::Vector2d(0.025, 0.225))); + const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), + CellLimits(6, 6)); const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); const std::vector discrete_scans = diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc index d554b4e..b17e513 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc @@ -232,8 +232,11 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( 1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction. M_PI, // Angular search window, 180 degrees in both directions. point_cloud, limits_.resolution()); - const transform::Rigid2d center = - transform::Rigid2d::Translation(limits_.edge_limits().center()); + const transform::Rigid2d center = transform::Rigid2d::Translation( + limits_.max() - + 0.5 * limits_.resolution() * + Eigen::Vector2d(limits_.cell_limits().num_y_cells, + limits_.cell_limits().num_x_cells)); return MatchWithSearchParameters(search_parameters, center, point_cloud, min_score, score, pose_estimate, covariance); } diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index 767f147..f9e635b 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -40,7 +40,7 @@ TEST(PrecomputationGridTest, CorrectValues) { std::mt19937 prng(42); std::uniform_int_distribution distribution(0, 255); ProbabilityGrid probability_grid( - MapLimits(0.05, 5., 5., CellLimits(250, 250))); + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { @@ -73,7 +73,8 @@ TEST(PrecomputationGridTest, CorrectValues) { TEST(PrecomputationGridTest, TinyProbabilityGrid) { std::mt19937 prng(42); std::uniform_int_distribution distribution(0, 255); - ProbabilityGrid probability_grid(MapLimits(0.05, 0.1, 0.1, CellLimits(4, 4))); + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(probability_grid.limits().cell_limits())) { @@ -147,7 +148,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { 0.5 * distribution(prng)); ProbabilityGrid probability_grid( - MapLimits(0.05, 5., 5., CellLimits(200, 200))); + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(), sensor::TransformPointCloud2D( @@ -198,7 +199,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { perturbation.inverse(); ProbabilityGrid probability_grid( - MapLimits(0.05, 5., 5., CellLimits(200, 200))); + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); laser_fan_inserter.Insert( sensor::LaserFan{ diff --git a/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h b/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h index baef23a..5932b0c 100644 --- a/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h +++ b/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h @@ -62,13 +62,12 @@ class OccupiedSpaceCostFunctor { const Eigen::Matrix point((T(point_cloud_[i].x())), (T(point_cloud_[i].y())), T(1.)); const Eigen::Matrix world = transform * point; - interpolator.Evaluate((limits.centered_limits().max().x() - world[0]) / - limits.resolution() + - T(kPadding), - (limits.centered_limits().max().y() - world[1]) / - limits.resolution() + - T(kPadding), - &residual[i]); + interpolator.Evaluate( + (limits.max().x() - world[0]) / limits.resolution() - 0.5 + + T(kPadding), + (limits.max().y() - world[1]) / limits.resolution() - 0.5 + + T(kPadding), + &residual[i]); residual[i] = scaling_factor_ * (1. - residual[i]); } return true; diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index 3c1a04a..aae6659 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -37,9 +37,8 @@ namespace { class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { protected: RealTimeCorrelativeScanMatcherTest() - : probability_grid_(MapLimits( - 0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025), - Eigen::Vector2d(0.025, 0.225)))) { + : probability_grid_( + MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) { { auto parameter_dictionary = common::MakeDictionary( "return { " diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index d9aa067..4525975 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -24,6 +24,7 @@ #include "Eigen/Geometry" #include "cartographer/common/make_unique.h" +#include "cartographer/common/port.h" #include "glog/logging.h" #include "webp/encode.h" @@ -70,11 +71,10 @@ ProbabilityGrid ComputeCroppedProbabilityGrid( CellLimits limits; probability_grid.ComputeCroppedLimits(&offset, &limits); const double resolution = probability_grid.limits().resolution(); - const double max_x = probability_grid.limits().centered_limits().max().x() - - resolution * offset.y(); - const double max_y = probability_grid.limits().centered_limits().max().y() - - resolution * offset.x(); - ProbabilityGrid cropped_grid(MapLimits(resolution, max_x, max_y, limits)); + const Eigen::Vector2d max = + probability_grid.limits().max() - + resolution * Eigen::Vector2d(offset.y(), offset.x()); + ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits)); cropped_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { if (probability_grid.IsKnown(xy_index + offset)) { @@ -164,13 +164,14 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) { if (size() > 1) { FinishSubmap(size() - 2); } + const int num_cells_per_dimension = + common::RoundToInt(2. * options_.half_length() / options_.resolution()) + + 1; submaps_.push_back(common::make_unique( MapLimits(options_.resolution(), - Eigen::AlignedBox2d( - origin.cast() - - options_.half_length() * Eigen::Vector2d::Ones(), - origin.cast() + - options_.half_length() * Eigen::Vector2d::Ones())), + origin.cast() + + options_.half_length() * Eigen::Vector2d::Ones(), + CellLimits(num_cells_per_dimension, num_cells_per_dimension)), origin, num_laser_fans_)); LOG(INFO) << "Added submap " << size(); num_laser_fans_in_last_submap_ = 0;