From 950d25762be002152f5100b4f9bce64206b94b57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 00:31:09 +0200 Subject: [PATCH] NavMap ObstacleFilter: mark the cell under each point MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The filter grouped the points in fixed 0.3 m columns and marked the NavCel under the column centre, up to about 0.21 m away from the obstacle. Each (already downsampled) point is now checked where it is: the NavCel under it, and its height above that NavCel. Fixes #114 Signed-off-by: Francisco Martín Rico --- .../filters/ObstacleFilter.cpp | 39 ++------------- .../tests/navmap_obstacle_filter_tests.cpp | 48 ++++++++++++++----- 2 files changed, 40 insertions(+), 47 deletions(-) diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index 0e2c7411..94ddf692 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include "easynav_common/Parameters.hpp" #include "easynav_common/types/NavState.hpp" @@ -100,34 +99,6 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) .fuse(tf_info.map_frame) .as_points(); - const float voxel_xy = 0.30f; - - struct Key - { - int ix, iy; - bool operator==(const Key & o) const noexcept {return ix == o.ix && iy == o.iy;} - }; - struct KeyHash - { - std::size_t operator()(const Key & k) const noexcept - { - std::size_t h1 = std::hash{}(static_cast(k.ix)); - std::size_t h2 = std::hash{}(static_cast(k.iy)); - return h1 ^ (h2 + 0x9e3779b97f4a7c15ULL + (h1 << 6) + (h1 >> 2)); - } - }; - - // Highest point of each column - std::unordered_map max_z; - max_z.reserve(points.size() / 4 + 1); - for (const auto & p : points.points) { - if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) {continue;} - const Key key{static_cast(std::floor(p.x / voxel_xy)), - static_cast(std::floor(p.y / voxel_xy))}; - auto [it, inserted] = max_z.try_emplace(key, p.z); - if (!inserted && p.z > it->second) {it->second = p.z;} - } - // The robot's position, for min_height_per_meter Eigen::Vector2f robot_xy(0.0f, 0.0f); bool robot_known = false; @@ -149,10 +120,10 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) std::optional last_surface; std::optional<::navmap::NavCelId> last_cid; - for (const auto & [key, top] : max_z) { - const float cx = (static_cast(key.ix) + 0.5f) * voxel_xy; - const float cy = (static_cast(key.iy) + 0.5f) * voxel_xy; - const Eigen::Vector3f query(cx, cy, top); + // Each point is checked where it is: the NavCel under it, and its height above that NavCel. + for (const auto & p : points.points) { + if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) {continue;} + const Eigen::Vector3f query(p.x, p.y, p.z); size_t surface_idx = 0; ::navmap::NavCelId cid; @@ -185,7 +156,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) if (normal.z() < 0.0f) {normal = -normal;} const float height = normal.dot(query - navmap_.positions.at(cel.v[0])); // Farther, a small attitude error lifts the ground more: a higher threshold - const double distance = robot_known ? (Eigen::Vector2f(cx, cy) - robot_xy).norm() : 0.0; + const double distance = robot_known ? (query.head<2>() - robot_xy).norm() : 0.0; if (!(height > min_height_ + min_height_per_meter_ * distance)) {continue;} navmap_.layer_set( diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp index a27f644d..cec33ddc 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp @@ -126,12 +126,11 @@ class NavmapObstacleFilterLimitsTest : public NavmapObstacleFilterTest } // Vertical column of points at (x, y) in the map, from z0 to z1. The filter marks the NavCel - // at the center of each 0.3 m voxel, so (x, y) are voxel centers; the points are put a bit - // off them, away from the voxel borders. + // under each point, so (x, y) are the centers of the 0.1 m grid cells checked. void column(float x, float y, float z0, float z1) { for (float z = z0; z <= z1 + 1e-3f; z += 0.1f) { - perception_.data.push_back(pcl::PointXYZ(x + 0.05f, y + 0.05f, z)); + perception_.data.push_back(pcl::PointXYZ(x, y, z)); } } @@ -143,7 +142,7 @@ class NavmapObstacleFilterLimitsTest : public NavmapObstacleFilterTest return nav_state_.get<::navmap::NavMap>("map.navmap"); } - // Highest value of the two triangles of the cell at (x, y): a voxel center lies on their + // Highest value of the two triangles of the cell at (x, y): a cell center lies on their // diagonal. static std::uint8_t cell_at(const ::navmap::NavMap & nm, float x, float y) { @@ -231,12 +230,17 @@ TEST_F(NavmapObstacleFilterLimitsTest, A2DLaserSeesObstacles) // A laser 0.095 m above the floor: all its points at one height node_->declare_parameter("navmap.obstacles.min_height", 0.05); for (float y = 3.9f; y <= 4.2f; y += 0.05f) { - perception_.data.push_back(pcl::PointXYZ(4.70f, y, 0.095f)); // a bin ahead + perception_.data.push_back(pcl::PointXYZ(4.65f, y, 0.095f)); // a bin ahead } - perception_.data.push_back(pcl::PointXYZ(5.60f, 4.10f, 0.095f)); // a single hit + perception_.data.push_back(pcl::PointXYZ(5.55f, 4.05f, 0.095f)); // a single hit easynav::navmap::ObstacleFilter filter; const auto & nm = run(filter); - EXPECT_EQ(cell_at(nm, 4.65f, 4.05f), navmap_ros::LETHAL_OBSTACLE); + // The bin: the cells under the points the 0.3 m downsampling keeps (one per voxel). + int bin_cells = 0; + for (float y = 3.85f; y <= 4.26f; y += 0.1f) { + bin_cells += cell_at(nm, 4.65f, y) == navmap_ros::LETHAL_OBSTACLE; + } + EXPECT_GE(bin_cells, 1); EXPECT_EQ(cell_at(nm, 5.55f, 4.05f), navmap_ros::LETHAL_OBSTACLE); } @@ -352,20 +356,20 @@ TEST_F(NavmapObstacleFilterLimitsTest, MinHeightGrowsWithTheDistanceToTheRobot) // Robot at (4, 4). Threshold 0.05 + 0.1 per meter: 0.12 m at 0.65 m, about 0.38 m at 3.3 m node_->declare_parameter("navmap.obstacles.min_height", 0.05); node_->declare_parameter("navmap.obstacles.min_height_per_meter", 0.1); - perception_.data.push_back(pcl::PointXYZ(4.70f, 4.10f, 0.2f)); // near, 0.2 m: obstacle - perception_.data.push_back(pcl::PointXYZ(7.40f, 4.10f, 0.2f)); // far, 0.2 m: the ground - perception_.data.push_back(pcl::PointXYZ(4.10f, 0.70f, 0.5f)); // far, 0.5 m: obstacle + perception_.data.push_back(pcl::PointXYZ(4.65f, 4.05f, 0.2f)); // near, 0.2 m: obstacle + perception_.data.push_back(pcl::PointXYZ(7.35f, 4.05f, 0.2f)); // far, 0.2 m: the ground + perception_.data.push_back(pcl::PointXYZ(4.05f, 0.75f, 0.5f)); // far, 0.5 m: obstacle easynav::navmap::ObstacleFilter filter; const auto & nm = run(filter); EXPECT_EQ(cell_at(nm, 4.65f, 4.05f), navmap_ros::LETHAL_OBSTACLE); EXPECT_EQ(cell_at(nm, 7.35f, 4.05f), navmap_ros::FREE_SPACE); - EXPECT_EQ(cell_at(nm, 4.05f, 0.75f), navmap_ros::LETHAL_OBSTACLE); // Its column's center + EXPECT_EQ(cell_at(nm, 4.05f, 0.75f), navmap_ros::LETHAL_OBSTACLE); } TEST_F(NavmapObstacleFilterLimitsTest, ByDefaultMinHeightDoesNotGrow) { node_->declare_parameter("navmap.obstacles.min_height", 0.05); - perception_.data.push_back(pcl::PointXYZ(7.40f, 4.10f, 0.095f)); // 3.35 m away, a laser hit + perception_.data.push_back(pcl::PointXYZ(7.35f, 4.05f, 0.095f)); // 3.35 m away, a laser hit easynav::navmap::ObstacleFilter filter; const auto & nm = run(filter); EXPECT_EQ(cell_at(nm, 7.35f, 4.05f), navmap_ros::LETHAL_OBSTACLE); @@ -377,7 +381,7 @@ TEST_F(NavmapObstacleFilterLimitsTest, AnInvalidMinHeightPerMeterIsZero) SetUp(); node_->declare_parameter("navmap.obstacles.min_height", 0.05); node_->declare_parameter("navmap.obstacles.min_height_per_meter", bad); - perception_.data.push_back(pcl::PointXYZ(7.40f, 4.10f, 0.095f)); + perception_.data.push_back(pcl::PointXYZ(7.35f, 4.05f, 0.095f)); easynav::navmap::ObstacleFilter filter; const auto & nm = run(filter); EXPECT_EQ(cell_at(nm, 7.35f, 4.05f), navmap_ros::LETHAL_OBSTACLE) << bad; @@ -399,3 +403,21 @@ TEST_F(NavmapObstacleFilterLimitsTest, WithoutTheRobotPoseMinHeightDoesNotGrow) nav_state_.set("scan", perception_); EXPECT_NO_THROW(filter.update(nav_state_)); } + +TEST_F(NavmapObstacleFilterLimitsTest, TheCellUnderThePointIsMarkedNotANeighbour) +{ + // A thin post at x = 4.71: only the 0.1 m cells it stands on are obstacles, not the ones + // around it (a 0.3 m column centre would fall at x = 4.65). No downsampling: both posts stay. + node_->declare_parameter("navmap.obstacles.downsample_resolution", 0.0); + easynav::navmap::ObstacleFilter filter; + for (float z = 0.3f; z <= 0.9f; z += 0.1f) { + perception_.data.push_back(pcl::PointXYZ(4.71f, 4.05f, z)); + perception_.data.push_back(pcl::PointXYZ(4.79f, 4.15f, z)); + } + const auto & nm = run(filter); + EXPECT_EQ(cell_at(nm, 4.75f, 4.05f), navmap_ros::LETHAL_OBSTACLE); + EXPECT_EQ(cell_at(nm, 4.75f, 4.15f), navmap_ros::LETHAL_OBSTACLE); + EXPECT_EQ(cell_at(nm, 4.65f, 4.05f), navmap_ros::FREE_SPACE); + EXPECT_EQ(cell_at(nm, 4.65f, 4.15f), navmap_ros::FREE_SPACE); + EXPECT_EQ(cell_at(nm, 4.85f, 4.05f), navmap_ros::FREE_SPACE); +}