Skip to content

NavMap ObstacleFilter: mark the cell under each point #117

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
fmrico merged 1 commit into from
Oct 15, 2026
Merged

NavMap ObstacleFilter: mark the cell under each point #117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 5 additions & 34 deletions ...rs/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
#include <cstdint>
#include <optional>
#include <string>
#include <unordered_map>

#include "easynav_common/Parameters.hpp"
#include "easynav_common/types/NavState.hpp"
Expand Down Expand Up @@ -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<int64_t>{}(static_cast<int64_t>(k.ix));
std::size_t h2 = std::hash<int64_t>{}(static_cast<int64_t>(k.iy));
return h1 ^ (h2 + 0x9e3779b97f4a7c15ULL + (h1 << 6) + (h1 >> 2));
}
};

// Highest point of each column
std::unordered_map<Key, float, KeyHash> 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<int>(std::floor(p.x / voxel_xy)),
static_cast<int>(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;
Expand All @@ -149,10 +120,10 @@ void ObstacleFilter::update(::easynav::NavState & nav_state)
std::optional<size_t> last_surface;
std::optional<::navmap::NavCelId> last_cid;

for (const auto & [key, top] : max_z) {
const float cx = (static_cast<float>(key.ix) + 0.5f) * voxel_xy;
const float cy = (static_cast<float>(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;
Expand Down Expand Up @@ -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<uint8_t>(
Expand Down
48 changes: 35 additions & 13 deletions maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}

Expand All @@ -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)
{
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand All @@ -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);
}