Generic landmark model for discrete detection sensors (both 2D and 3D). More...
#include <landmark_sensor_model.hpp>
Public Types | |
using | map_type = LandmarkMap |
Map representation type. More... | |
using | measurement_type = std::vector< LandmarkPositionDetection > |
Measurement type of the sensor, detection position in robot frame. More... | |
using | param_type = LandmarkModelParam |
Parameter type that the constructor uses to configure the beam sensor model. More... | |
using | state_type = StateType |
State type of a particle. More... | |
using | weight_type = double |
Weight type of the particle. More... | |
Public Member Functions | |
LandmarkSensorModel (param_type params, LandmarkMap landmark_map) | |
Constructs a LandmarkSensorModel instance. More... | |
auto | operator() (measurement_type &&detections) const |
Returns a state weighting function conditioned on landmark position detections. More... | |
void | update_map (map_type &&map) |
Update the sensor model with a new landmark map . More... | |
Private Attributes | |
LandmarkMap | landmark_map_ |
param_type | params_ |
Generic landmark model for discrete detection sensors (both 2D and 3D).
This sensor model is a generalization of the Sensor Model with Known Correspondences described in Probabilistic Robotics [thrun2005probabilistic], Chapter 6.6. This version has been extended to support landmark detection in 3D even when the robot is constrained to 2D motion. The importance weight function models detections as having two components that can be independently measured: range and bearing.
LandmarkMap | class managing the list of known landmarks. |
StateType | type of the state of the particle. |
Definition at line 63 of file landmark_sensor_model.hpp.
using beluga::LandmarkSensorModel< LandmarkMap, StateType >::map_type = LandmarkMap |
Map representation type.
Definition at line 72 of file landmark_sensor_model.hpp.
using beluga::LandmarkSensorModel< LandmarkMap, StateType >::measurement_type = std::vector<LandmarkPositionDetection> |
Measurement type of the sensor, detection position in robot frame.
Definition at line 70 of file landmark_sensor_model.hpp.
using beluga::LandmarkSensorModel< LandmarkMap, StateType >::param_type = LandmarkModelParam |
Parameter type that the constructor uses to configure the beam sensor model.
Definition at line 74 of file landmark_sensor_model.hpp.
using beluga::LandmarkSensorModel< LandmarkMap, StateType >::state_type = StateType |
State type of a particle.
Definition at line 66 of file landmark_sensor_model.hpp.
using beluga::LandmarkSensorModel< LandmarkMap, StateType >::weight_type = double |
Weight type of the particle.
Definition at line 68 of file landmark_sensor_model.hpp.
|
inlineexplicit |
Constructs a LandmarkSensorModel instance.
params | Parameters to configure this instance. See beluga::BeamModelParams for details. |
landmark_map | Map of landmarks to be used by the sensor model to compute importance weights for particle states. |
Definition at line 82 of file landmark_sensor_model.hpp.
|
inline |
Returns a state weighting function conditioned on landmark position detections.
detections | 2D lidar hit points in the reference frame of filter particles. |
Definition at line 91 of file landmark_sensor_model.hpp.
|
inline |
Update the sensor model with a new landmark map
.
Definition at line 159 of file landmark_sensor_model.hpp.
|
private |
Definition at line 163 of file landmark_sensor_model.hpp.
|
private |
Definition at line 162 of file landmark_sensor_model.hpp.