Fix a bug introduced in the recent clean up. (#13)

cartographer-project · Aug 26, 2016 · 5b20df7 · 5b20df7
1 parent 0c9f416
commit 5b20df7
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 3 deletions.
diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h
@@ -89,9 +89,11 @@ class MapLimits {
     const float kPadding = 3.f * resolution;
     bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
     bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
+    const Eigen::Vector2d pixel_sizes =
+        bounding_box.sizes().cast<double>() / resolution;
     return MapLimits(resolution, bounding_box.max().cast<double>(),
-                     CellLimits(common::RoundToInt(bounding_box.sizes().y()),
-                                common::RoundToInt(bounding_box.sizes().x())));
+                     CellLimits(common::RoundToInt(pixel_sizes.y()),
+                                common::RoundToInt(pixel_sizes.x())));
   }
 
   static Eigen::AlignedBox2f ComputeMapBoundingBox(

diff --git a/cartographer/mapping_2d/map_limits_test.cc b/cartographer/mapping_2d/map_limits_test.cc
@@ -22,7 +22,7 @@ namespace cartographer {
 namespace mapping_2d {
 namespace {
 
-TEST(MapLimits, ConstructAndGet) {
+TEST(MapLimitsTest, ConstructAndGet) {
   const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3));
 
   const CellLimits& cell_limits = limits.cell_limits();
@@ -36,6 +36,29 @@ TEST(MapLimits, ConstructAndGet) {
   EXPECT_EQ(42., limits.resolution());
 }
 
+TEST(MapLimitsTest, ComputeMapLimits) {
+  const mapping::TrajectoryNode::ConstantData constant_data{
+      common::FromUniversal(52),
+      sensor::LaserFan{
+          Eigen::Vector2f::Zero(),
+          {Eigen::Vector2f(-30.f, 1.f), Eigen::Vector2f(50.f, -10.f)},
+          {}},
+      Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}),
+      nullptr, transform::Rigid3d::Identity()};
+  const mapping::TrajectoryNode trajectory_node{&constant_data,
+                                                transform::Rigid3d::Identity()};
+  constexpr double kResolution = 0.05;
+  const MapLimits limits =
+      MapLimits::ComputeMapLimits(kResolution, {trajectory_node});
+  constexpr float kPaddingAwareTolerance = 5 * kResolution;
+  EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance);
+  EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance);
+  EXPECT_LT(200, limits.cell_limits().num_x_cells);
+  EXPECT_LT(1600, limits.cell_limits().num_y_cells);
+  EXPECT_GT(400, limits.cell_limits().num_x_cells);
+  EXPECT_GT(2000, limits.cell_limits().num_y_cells);
+}
+
 }  // namespace
 }  // namespace mapping_2d
 }  // namespace cartographer