Template Class BeamSensorModel

Class Documentation

template<class OccupancyGrid>
class BeamSensorModel

Beam sensor model for range finders.

This model closely matches the physical behavior of a lidar, but it is computationally expensive because a ray-tracing operation needs to be performed for each particle and individual range reading. Also, this model can experience degraded performance in cluttered environments due the inherent lack of smoothness of the sensor readings in such settings. See Probabilistic Robotics thrun2005probabilistic Chapter 6.2 for further reference.

Note

This class satisfies Beluga named requirements: SensorModel.

Template Parameters:

OccupancyGrid – Type representing an occupancy grid. It must satisfy Beluga named requirements: OccupancyGrid2d.

Public Types

using state_type = Sophus::SE2d

State type of a particle.

using weight_type = double

Weight type of the particle.

using measurement_type = std::vector<std::pair<double, double>>

Measurement type of the sensor: a point cloud for the range finder.

using map_type = OccupancyGrid

Map representation type.

using param_type = BeamModelParam

Parameter type that the constructor uses to configure the beam sensor model.

Public Functions

inline explicit BeamSensorModel(const param_type &params, OccupancyGrid grid)

Constructs a BeamSensorModel instance.

Parameters:
  • params – Parameters to configure this instance. See beluga::BeamModelParams for details.

  • grid – Occupancy grid representing the static map that the sensor model uses to raytrace lidar beams and compute importance weights for particle states.

inline auto operator()(measurement_type &&points) const

Returns a state weighting function conditioned on 2D lidar hits.

Parameters:

points – 2D lidar hit points in the reference frame of particle states.

Returns:

a state weighting function satisfying Beluga named requirements: StateWeightingFunction and borrowing a reference to this sensor model (and thus their lifetime are bound).

inline void update_map(map_type &&map)

Update the sensor model with a new occupancy grid map.

Parameters:

map – New occupancy grid representing the static map.