diff --git a/beluga_amcl/include/beluga_amcl/models.hpp b/beluga_amcl/include/beluga_amcl/models.hpp index 3856909a0..cbfbbd1d6 100644 --- a/beluga_amcl/include/beluga_amcl/models.hpp +++ b/beluga_amcl/include/beluga_amcl/models.hpp @@ -163,13 +163,12 @@ class LikelihoodSensorModel : public Mixin }); auto squared_distance = - [height = map.info.height, - width = map.info.width, + [width = map.info.width, squared_resolution = map.info.resolution * map.info.resolution, squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance ](std::size_t first, std::size_t second) { auto delta_x = - static_cast(first % height) - static_cast(second % height); + static_cast(first % width) - static_cast(second % width); auto delta_y = static_cast(first / width) - static_cast(second / width); return std::min( @@ -179,16 +178,16 @@ class LikelihoodSensorModel : public Mixin auto neighbors = [&map](std::size_t index) { auto result = std::vector{}; - if (index / map.info.width < map.info.height - 1) { - result.push_back(index + map.info.height); + if (index / map.info.width < (map.info.height - 1)) { + result.push_back(index + map.info.width); } if (index / map.info.width > 0) { - result.push_back(index - map.info.height); + result.push_back(index - map.info.width); } - if (index % map.info.height < map.info.width - 1) { + if (index % map.info.width < (map.info.width - 1)) { result.push_back(index + 1); } - if (index % map.info.height > 0) { + if (index % map.info.width > 0) { result.push_back(index - 1); } return result; @@ -210,7 +209,7 @@ class LikelihoodSensorModel : public Mixin std::size_t index) { return std::make_pair( - (static_cast(index % info.height) + 0.5) * info.resolution + info.origin.position.x, + (static_cast(index % info.width) + 0.5) * info.resolution + info.origin.position.x, (static_cast(index / info.width) + 0.5) * info.resolution + info.origin.position.y); } };