Sampled odometry model for a differential drive. More...
#include <differential_drive_model.hpp>
Public Types | |
| using | control_type = std::tuple< state_type, state_type > |
| Current and previous odometry estimates as motion model control action. More... | |
| using | param_type = DifferentialDriveModelParam |
| Parameter type that the constructor uses to configure the motion model. More... | |
| using | state_type = StateType |
| 2D or flattened 3D pose as motion model state (to match that of the particles). More... | |
Public Member Functions | |
| DifferentialDriveModel (const param_type ¶ms) | |
| Constructs a DifferentialDriveModel instance. More... | |
| template<class Control , typename = common_tuple_type_t<Control, control_type>> | |
| auto | operator() (const Control &action) const |
| Computes a state sampling function conditioned on a given control action. More... | |
Private Types | |
| using | control_type_2d = std::tuple< Sophus::SE2d, Sophus::SE2d > |
| using | control_type_3d = std::tuple< Sophus::SE3d, Sophus::SE3d > |
Private Member Functions | |
| auto | sampling_fn_2d (const Sophus::SE2d &pose, const Sophus::SE2d &previous_pose) const |
| auto | sampling_fn_3d (const Sophus::SE3d &pose, const Sophus::SE3d &previous_pose) const |
Static Private Member Functions | |
| static double | rotation_variance (const Sophus::SO2d &rotation) |
Private Attributes | |
| param_type | params_ |
Sampled odometry model for a differential drive.
Supports 2D and (flattened) 3D state types. This class satisfies Beluga named requirements: MotionModel.
See Probabilistic Robotics [thrun2005probabilistic] Chapter 5.4.2.
| StateType | Type for particle's state. Either Sophus::SE2d or Sophus::SE3d. |
Definition at line 80 of file differential_drive_model.hpp.
| using beluga::DifferentialDriveModel< StateType >::control_type = std::tuple<state_type, state_type> |
Current and previous odometry estimates as motion model control action.
Definition at line 90 of file differential_drive_model.hpp.
|
private |
Definition at line 119 of file differential_drive_model.hpp.
|
private |
Definition at line 120 of file differential_drive_model.hpp.
| using beluga::DifferentialDriveModel< StateType >::param_type = DifferentialDriveModelParam |
Parameter type that the constructor uses to configure the motion model.
Definition at line 93 of file differential_drive_model.hpp.
| using beluga::DifferentialDriveModel< StateType >::state_type = StateType |
2D or flattened 3D pose as motion model state (to match that of the particles).
Definition at line 87 of file differential_drive_model.hpp.
|
inlineexplicit |
Constructs a DifferentialDriveModel instance.
| params | Parameters to configure this instance. See beluga::DifferentialDriveModelParam for details. |
Definition at line 100 of file differential_drive_model.hpp.
|
inline |
Computes a state sampling function conditioned on a given control action.
| Control | A tuple-like container matching the model's control_type. |
| action | Control action to condition the motion model with. |
Definition at line 109 of file differential_drive_model.hpp.
|
inlinestaticprivate |
Definition at line 167 of file differential_drive_model.hpp.
|
inlineprivate |
Definition at line 129 of file differential_drive_model.hpp.
|
inlineprivate |
Definition at line 122 of file differential_drive_model.hpp.
|
private |
Definition at line 165 of file differential_drive_model.hpp.