class mp2p_icp_filters::FilterTraversability

Overview

Finds the obstacles for a ground vehicle in a 3D point cloud (e.g. a whole map): steep slopes, steps and drop-offs, and anything at the height of the vehicle body above the local ground. Gentle slopes remain traversable.

The input cloud (map frame, z up) is analyzed on a 2D grid of resolution :

1. Ground model : the lowest point of each cell is its ground, if it is not too far above the lowest points around it (ground_envelope_size, ground_envelope_slope_deg): cells where only treetops, wall or table tops were seen get their ground interpolated from neighboring ground, up to ground_fill_distance. Never observed cells within that distance get interpolated ground too, so sparse ground stays continuous: the edge of a drop-off whose lower side was never seen is placed up to ground_fill_distance past the real edge (reduce it if that matters).

2. Per cell checks : a cell is not traversable if the slope of a plane fitted to the ground within slope_window_radius exceeds max_slope_deg, if the ground height changes by more than max_step to a neighbor cell, or if it has at least body_min_points points between body_min_height and body_max_height above its ground (overhangs higher than the vehicle are ignored).

3. Reachability (only if seeds_pointcloud_layer is given, e.g. the trajectory followed while mapping, see FilterInsertPoint): the traversable region is grown from the seeds, which are traversable by definition within seeds_radius, without crossing steps. The obstacles are then the cells around the reachable region: its border (including never observed cells if unknown_is_obstacle, e.g. the hidden side of a drop-off) and all non traversable cells up to obstacle_distance from it. Without seeds, all non traversable observed cells are obstacles.

Outputs are points at the cell centers, at their ground height: output_layer_obstacles and, optionally, output_layer_traversable (the reachable cells, or the traversable ones without seeds).

The grid is dense over the bounding box of the input: use a coarser resolution for very large maps.

#include <FilterTraversability.h>

class FilterTraversability: public mp2p_icp_filters::FilterBase
{
public:
    // structs

    struct Parameters;

    // fields

    Parameters params;

    // methods

    virtual void filter(mp2p_icp::metric_map_t& inOut) const;
};

Inherited Members

public:
    // methods

    Parameterizable& operator = (const Parameterizable&);
    Parameterizable& operator = (Parameterizable&&);
    virtual void filter(mp2p_icp::metric_map_t& inOut) const = 0;
    FilterBase& operator = (const FilterBase&);
    FilterBase& operator = (FilterBase&&);

Fields

Parameters params

Algorithm parameters

Methods

virtual void filter(mp2p_icp::metric_map_t& inOut) const

See docs above for FilterBase.