Beam sensor model for range finders. More...
#include <beam_model.hpp>
Public Types | |
using | map_type = OccupancyGrid |
Map representation type. More... | |
using | measurement_type = std::vector< std::pair< double, double > > |
Measurement type of the sensor: a point cloud for the range finder. More... | |
using | param_type = BeamModelParam |
Parameter type that the constructor uses to configure the beam sensor model. More... | |
using | state_type = Sophus::SE2d |
State type of a particle. More... | |
using | weight_type = double |
Weight type of the particle. More... | |
Public Member Functions | |
BeamSensorModel (const param_type ¶ms, OccupancyGrid grid) | |
Constructs a BeamSensorModel instance. More... | |
auto | operator() (measurement_type &&points) const |
Returns a state weighting function conditioned on 2D lidar hits. More... | |
void | update_map (map_type &&map) |
Update the sensor model with a new occupancy grid map. More... | |
Private Attributes | |
OccupancyGrid | grid_ |
param_type | params_ |
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.
OccupancyGrid | Type representing an occupancy grid. It must satisfy Beluga named requirements: OccupancyGrid2d. |
Definition at line 75 of file beam_model.hpp.
using beluga::BeamSensorModel< OccupancyGrid >::map_type = OccupancyGrid |
Map representation type.
Definition at line 84 of file beam_model.hpp.
using beluga::BeamSensorModel< OccupancyGrid >::measurement_type = std::vector<std::pair<double, double> > |
Measurement type of the sensor: a point cloud for the range finder.
Definition at line 82 of file beam_model.hpp.
using beluga::BeamSensorModel< OccupancyGrid >::param_type = BeamModelParam |
Parameter type that the constructor uses to configure the beam sensor model.
Definition at line 86 of file beam_model.hpp.
using beluga::BeamSensorModel< OccupancyGrid >::state_type = Sophus::SE2d |
State type of a particle.
Definition at line 78 of file beam_model.hpp.
using beluga::BeamSensorModel< OccupancyGrid >::weight_type = double |
Weight type of the particle.
Definition at line 80 of file beam_model.hpp.
|
inlineexplicit |
Constructs a BeamSensorModel instance.
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. |
Definition at line 95 of file beam_model.hpp.
|
inline |
Returns a state weighting function conditioned on 2D lidar hits.
points | 2D lidar hit points in the reference frame of particle states. |
Definition at line 103 of file beam_model.hpp.
|
inline |
Update the sensor model with a new occupancy grid map.
map | New occupancy grid representing the static map. |
Definition at line 155 of file beam_model.hpp.
|
private |
Definition at line 159 of file beam_model.hpp.
|
private |
Definition at line 158 of file beam_model.hpp.