diff --git a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc index f94ce4e..a4934f1 100644 --- a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc @@ -21,12 +21,79 @@ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/mapping/2d/xy_index.h" -#include "cartographer/mapping/internal/2d/ray_casting.h" +#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" #include "cartographer/mapping/probability_values.h" #include "glog/logging.h" namespace cartographer { namespace mapping { +namespace { + +// Factor for subpixel accuracy of start and end point for ray casts. +constexpr int kSubpixelScale = 1000; + +void GrowAsNeeded(const sensor::RangeData& range_data, + ProbabilityGrid* const probability_grid) { + Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); + constexpr float kPadding = 1e-6f; + for (const Eigen::Vector3f& hit : range_data.returns) { + bounding_box.extend(hit.head<2>()); + } + for (const Eigen::Vector3f& miss : range_data.misses) { + bounding_box.extend(miss.head<2>()); + } + probability_grid->GrowLimits(bounding_box.min() - + kPadding * Eigen::Vector2f::Ones()); + probability_grid->GrowLimits(bounding_box.max() + + kPadding * Eigen::Vector2f::Ones()); +} + +void CastRays(const sensor::RangeData& range_data, + const std::vector& hit_table, + const std::vector& miss_table, + const bool insert_free_space, ProbabilityGrid* probability_grid) { + GrowAsNeeded(range_data, probability_grid); + + const MapLimits& limits = probability_grid->limits(); + const double superscaled_resolution = limits.resolution() / kSubpixelScale; + const MapLimits superscaled_limits( + superscaled_resolution, limits.max(), + CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, + limits.cell_limits().num_y_cells * kSubpixelScale)); + const Eigen::Array2i begin = + superscaled_limits.GetCellIndex(range_data.origin.head<2>()); + // Compute and add the end points. + std::vector ends; + ends.reserve(range_data.returns.size()); + for (const Eigen::Vector3f& hit : range_data.returns) { + ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>())); + probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); + } + + if (!insert_free_space) { + return; + } + + // Now add the misses. + for (const Eigen::Array2i& end : ends) { + std::vector ray = + RayToPixelMask(begin, end, kSubpixelScale); + for (const Eigen::Array2i& cell_index : ray) { + probability_grid->ApplyLookupTable(cell_index, miss_table); + } + } + + // Finally, compute and add empty rays based on misses in the range data. + for (const Eigen::Vector3f& missing_echo : range_data.misses) { + std::vector ray = RayToPixelMask( + begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), + kSubpixelScale); + for (const Eigen::Array2i& cell_index : ray) { + probability_grid->ApplyLookupTable(cell_index, miss_table); + } + } +} +} // namespace proto::ProbabilityGridRangeDataInserterOptions2D CreateProbabilityGridRangeDataInserterOptions2D( diff --git a/cartographer/mapping/internal/2d/ray_casting.cc b/cartographer/mapping/internal/2d/ray_casting.cc deleted file mode 100644 index 33d8d81..0000000 --- a/cartographer/mapping/internal/2d/ray_casting.cc +++ /dev/null @@ -1,206 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/mapping/internal/2d/ray_casting.h" - -namespace cartographer { -namespace mapping { -namespace { - -// Factor for subpixel accuracy of start and end point. -constexpr int kSubpixelScale = 1000; - -// We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin' -// and 'end' are coordinates at subpixel precision. We compute all pixels in -// which some part of the line segment connecting 'begin' and 'end' lies. -void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, - const std::vector& miss_table, - ProbabilityGrid* const probability_grid) { - // For simplicity, we order 'begin' and 'end' by their x coordinate. - if (begin.x() > end.x()) { - CastRay(end, begin, miss_table, probability_grid); - return; - } - - CHECK_GE(begin.x(), 0); - CHECK_GE(begin.y(), 0); - CHECK_GE(end.y(), 0); - - // Special case: We have to draw a vertical line in full pixels, as 'begin' - // and 'end' have the same full pixel x coordinate. - if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) { - Eigen::Array2i current(begin.x() / kSubpixelScale, - std::min(begin.y(), end.y()) / kSubpixelScale); - const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale; - for (; current.y() <= end_y; ++current.y()) { - probability_grid->ApplyLookupTable(current, miss_table); - } - return; - } - - const int64 dx = end.x() - begin.x(); - const int64 dy = end.y() - begin.y(); - const int64 denominator = 2 * kSubpixelScale * dx; - - // The current full pixel coordinates. We begin at 'begin'. - Eigen::Array2i current = begin / kSubpixelScale; - - // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in - // the denominator. - // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale) - // | | | | - // +-+-+-+ - // | | | | - // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale) - // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale) - // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale) - - // The center of the subpixel part of 'begin.y()' assuming the - // 'denominator', i.e., sub_y / denominator is in (0, 1). - int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx; - - // The distance from the from 'begin' to the right pixel border, to be divided - // by 2 * 'kSubpixelScale'. - const int first_pixel = - 2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1; - // The same from the left pixel border to 'end'. - const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1; - - // The full pixel x coordinate of 'end'. - const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale; - - // Move from 'begin' to the next pixel border to the right. - sub_y += dy * first_pixel; - if (dy > 0) { - while (true) { - probability_grid->ApplyLookupTable(current, miss_table); - while (sub_y > denominator) { - sub_y -= denominator; - ++current.y(); - probability_grid->ApplyLookupTable(current, miss_table); - } - ++current.x(); - if (sub_y == denominator) { - sub_y -= denominator; - ++current.y(); - } - if (current.x() == end_x) { - break; - } - // Move from one pixel border to the next. - sub_y += dy * 2 * kSubpixelScale; - } - // Move from the pixel border on the right to 'end'. - sub_y += dy * last_pixel; - probability_grid->ApplyLookupTable(current, miss_table); - while (sub_y > denominator) { - sub_y -= denominator; - ++current.y(); - probability_grid->ApplyLookupTable(current, miss_table); - } - CHECK_NE(sub_y, denominator); - CHECK_EQ(current.y(), end.y() / kSubpixelScale); - return; - } - - // Same for lines non-ascending in y coordinates. - while (true) { - probability_grid->ApplyLookupTable(current, miss_table); - while (sub_y < 0) { - sub_y += denominator; - --current.y(); - probability_grid->ApplyLookupTable(current, miss_table); - } - ++current.x(); - if (sub_y == 0) { - sub_y += denominator; - --current.y(); - } - if (current.x() == end_x) { - break; - } - sub_y += dy * 2 * kSubpixelScale; - } - sub_y += dy * last_pixel; - probability_grid->ApplyLookupTable(current, miss_table); - while (sub_y < 0) { - sub_y += denominator; - --current.y(); - probability_grid->ApplyLookupTable(current, miss_table); - } - CHECK_NE(sub_y, 0); - CHECK_EQ(current.y(), end.y() / kSubpixelScale); -} - -void GrowAsNeeded(const sensor::RangeData& range_data, - ProbabilityGrid* const probability_grid) { - Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); - constexpr float kPadding = 1e-6f; - for (const Eigen::Vector3f& hit : range_data.returns) { - bounding_box.extend(hit.head<2>()); - } - for (const Eigen::Vector3f& miss : range_data.misses) { - bounding_box.extend(miss.head<2>()); - } - probability_grid->GrowLimits(bounding_box.min() - - kPadding * Eigen::Vector2f::Ones()); - probability_grid->GrowLimits(bounding_box.max() + - kPadding * Eigen::Vector2f::Ones()); -} - -} // namespace - -void CastRays(const sensor::RangeData& range_data, - const std::vector& hit_table, - const std::vector& miss_table, - const bool insert_free_space, - ProbabilityGrid* const probability_grid) { - GrowAsNeeded(range_data, probability_grid); - - const MapLimits& limits = probability_grid->limits(); - const double superscaled_resolution = limits.resolution() / kSubpixelScale; - const MapLimits superscaled_limits( - superscaled_resolution, limits.max(), - CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, - limits.cell_limits().num_y_cells * kSubpixelScale)); - const Eigen::Array2i begin = - superscaled_limits.GetCellIndex(range_data.origin.head<2>()); - // Compute and add the end points. - std::vector ends; - ends.reserve(range_data.returns.size()); - for (const Eigen::Vector3f& hit : range_data.returns) { - ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>())); - probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); - } - - if (!insert_free_space) { - return; - } - - // Now add the misses. - for (const Eigen::Array2i& end : ends) { - CastRay(begin, end, miss_table, probability_grid); - } - - // Finally, compute and add empty rays based on misses in the range data. - for (const Eigen::Vector3f& missing_echo : range_data.misses) { - CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), - miss_table, probability_grid); - } -} - -} // namespace mapping -} // namespace cartographer diff --git a/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc b/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc new file mode 100644 index 0000000..643825c --- /dev/null +++ b/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc @@ -0,0 +1,159 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" + +#include "Eigen/Dense" + +namespace cartographer { +namespace mapping { +namespace { + +bool isEqual(const Eigen::Array2i& lhs, const Eigen::Array2i& rhs) { + return ((lhs - rhs).matrix().lpNorm<1>() == 0); +} +} // namespace + +// Compute all pixels that contain some part of the line segment connecting +// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled +// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be +// greater than zero. Return values are in pixels and not scaled. +std::vector RayToPixelMask(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale) { + // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x + // coordinate. + if (scaled_begin.x() > scaled_end.x()) { + return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale); + } + + CHECK_GE(scaled_begin.x(), 0); + CHECK_GE(scaled_begin.y(), 0); + CHECK_GE(scaled_end.y(), 0); + std::vector pixel_mask; + // Special case: We have to draw a vertical line in full pixels, as + // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate. + if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) { + Eigen::Array2i current( + scaled_begin.x() / subpixel_scale, + std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale); + pixel_mask.push_back(current); + const int end_y = + std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale; + for (; current.y() <= end_y; ++current.y()) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + return pixel_mask; + } + + const int64 dx = scaled_end.x() - scaled_begin.x(); + const int64 dy = scaled_end.y() - scaled_begin.y(); + const int64 denominator = 2 * subpixel_scale * dx; + + // The current full pixel coordinates. We scaled_begin at 'scaled_begin'. + Eigen::Array2i current = scaled_begin / subpixel_scale; + pixel_mask.push_back(current); + + // To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in + // the denominator. + // +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale) + // | | | | + // +-+-+-+ + // | | | | + // +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale) + // | | | | -- center of first subpixel = 1 / (2 * subpixel_scale) + // +-+-+-+ -- 0 = 0 / (2 * subpixel_scale) + + // The center of the subpixel part of 'scaled_begin.y()' assuming the + // 'denominator', i.e., sub_y / denominator is in (0, 1). + int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx; + + // The distance from the from 'scaled_begin' to the right pixel border, to be + // divided by 2 * 'subpixel_scale'. + const int first_pixel = + 2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1; + // The same from the left pixel border to 'scaled_end'. + const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1; + + // The full pixel x coordinate of 'scaled_end'. + const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale; + + // Move from 'scaled_begin' to the next pixel border to the right. + sub_y += dy * first_pixel; + if (dy > 0) { + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + ++current.x(); + if (sub_y == denominator) { + sub_y -= denominator; + ++current.y(); + } + if (current.x() == end_x) { + break; + } + // Move from one pixel border to the next. + sub_y += dy * 2 * subpixel_scale; + } + // Move from the pixel border on the right to 'scaled_end'. + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, denominator); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + return pixel_mask; + } + + // Same for lines non-ascending in y coordinates. + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + ++current.x(); + if (sub_y == 0) { + sub_y += denominator; + --current.y(); + } + if (current.x() == end_x) { + break; + } + sub_y += dy * 2 * subpixel_scale; + } + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, 0); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + return pixel_mask; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/2d/ray_casting.h b/cartographer/mapping/internal/2d/ray_to_pixel_mask.h similarity index 52% rename from cartographer/mapping/internal/2d/ray_casting.h rename to cartographer/mapping/internal/2d/ray_to_pixel_mask.h index ae89a0b..16b9a72 100644 --- a/cartographer/mapping/internal/2d/ray_casting.h +++ b/cartographer/mapping/internal/2d/ray_to_pixel_mask.h @@ -14,28 +14,25 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ #include -#include "cartographer/common/port.h" -#include "cartographer/mapping/2d/probability_grid.h" -#include "cartographer/sensor/point_cloud.h" -#include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping { -// For each ray in 'range_data', inserts hits and misses into -// 'probability_grid'. Hits are handled before misses. -void CastRays(const sensor::RangeData& range_data, - const std::vector& hit_table, - const std::vector& miss_table, bool insert_free_space, - ProbabilityGrid* probability_grid); +// Compute all pixels that contain some part of the line segment connecting +// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled +// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be +// greater than zero. Return values are in pixels and not scaled. +std::vector RayToPixelMask(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale); } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ diff --git a/cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc b/cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc new file mode 100644 index 0000000..bfc7650 --- /dev/null +++ b/cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc @@ -0,0 +1,207 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" + +#include "cartographer/mapping/2d/map_limits.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using ::testing::ElementsAre; + +MATCHER_P(PixelMaskEqual, value, "") { + Eigen::Array2i residual = value - arg; + return residual.matrix().lpNorm<1>() == 0; +} + +TEST(RayToPixelMaskTest, SingleCell) { + const Eigen::Array2i& begin = {1, 1}; + const Eigen::Array2i& end = {1, 1}; + const int subpixel_scale = 1; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + std::vector ray_reference = + std::vector{begin}; + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(begin))); +} + +TEST(RayToPixelMaskTest, AxisAlignedX) { + const Eigen::Array2i& begin = {1, 1}; + const Eigen::Array2i& end = {3, 1}; + const int subpixel_scale = 1; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); +} + +TEST(RayToPixelMaskTest, AxisAlignedY) { + const Eigen::Array2i& begin = {1, 1}; + const Eigen::Array2i& end = {1, 3}; + const int subpixel_scale = 1; + std::vector ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3})}; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({1, 3})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({1, 3})))); +} + +TEST(RayToPixelMaskTest, Diagonal) { + Eigen::Array2i begin = {1, 1}; + Eigen::Array2i end = {3, 3}; + const int subpixel_scale = 1; + std::vector ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 3})}; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 3})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 3})))); + begin = Eigen::Array2i({1, 3}); + end = Eigen::Array2i({3, 1}); + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); +} + +TEST(RayToPixelMaskTest, SteepLine) { + Eigen::Array2i begin = {1, 1}; + Eigen::Array2i end = {2, 5}; + const int subpixel_scale = 1; + std::vector ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3}), + Eigen::Array2i({2, 3}), Eigen::Array2i({2, 4}), Eigen::Array2i({2, 5})}; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({1, 3})), + PixelMaskEqual(Eigen::Array2i({2, 3})), + PixelMaskEqual(Eigen::Array2i({2, 4})), + PixelMaskEqual(Eigen::Array2i({2, 5})))); + begin = {1, 1}; + end = {2, 4}; + ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({2, 3}), + Eigen::Array2i({2, 4})}; + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({2, 3})), + PixelMaskEqual(Eigen::Array2i({2, 4})))); +} + +TEST(RayToPixelMaskTest, FlatLine) { + Eigen::Array2i begin = {1, 1}; + Eigen::Array2i end = {5, 2}; + const int subpixel_scale = 1; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 1})), + PixelMaskEqual(Eigen::Array2i({3, 2})), + PixelMaskEqual(Eigen::Array2i({4, 2})), + PixelMaskEqual(Eigen::Array2i({5, 2})))); + begin = {1, 1}; + end = {4, 2}; + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 2})), + PixelMaskEqual(Eigen::Array2i({4, 2})))); +} + +TEST(RayToPixelMaskTest, MultiScaleAxisAlignedX) { + int subpixel_scale; + const int num_cells_x = 20; + const int num_cells_y = 20; + double resolution = 0.1; + Eigen::Vector2d max = {1.0, 1.0}; + std::vector base_scale_ray; + for (subpixel_scale = 1; subpixel_scale < 10000; subpixel_scale *= 2) { + double superscaled_resolution = resolution / subpixel_scale; + MapLimits superscaled_limits( + superscaled_resolution, max, + CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale)); + Eigen::Array2i begin = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.05, 0.05})); + Eigen::Array2i end = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.35, 0.05})); + std::vector ray = + RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({9, 6})), + PixelMaskEqual(Eigen::Array2i({9, 7})), + PixelMaskEqual(Eigen::Array2i({9, 8})), + PixelMaskEqual(Eigen::Array2i({9, 9})))); + } +} + +TEST(RayToPixelMaskTest, MultiScaleSkewedLine) { + int subpixel_scale = 1; + const int num_cells_x = 20; + const int num_cells_y = 20; + double resolution = 0.1; + Eigen::Vector2d max = {1.0, 1.0}; + double superscaled_resolution = resolution / subpixel_scale; + MapLimits superscaled_limits( + superscaled_resolution, max, + CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale)); + Eigen::Array2i begin = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09})); + Eigen::Array2i end = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19})); + + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})), + PixelMaskEqual(Eigen::Array2i({8, 8})), + PixelMaskEqual(Eigen::Array2i({9, 8})), + PixelMaskEqual(Eigen::Array2i({9, 9})))); + subpixel_scale = 20; + superscaled_resolution = resolution / subpixel_scale; + superscaled_limits = MapLimits( + superscaled_resolution, max, + CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale)); + begin = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09})); + end = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19})); + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})), + PixelMaskEqual(Eigen::Array2i({8, 8})), + PixelMaskEqual(Eigen::Array2i({8, 9})), + PixelMaskEqual(Eigen::Array2i({9, 9})))); +} + +} // namespace +} // namespace mapping +} // namespace cartographer