#include <MotionModel.h>
Public Member Functions | |
void | applyOdomTransform (Particles &particles, const tf::Transform &odomTransform) |
apply odomTransform to all particles (with random noise and calibration) | |
bool | applyOdomTransformTemporal (Particles &particles, const ros::Time &t, double dt) |
tf::Transform | computeOdomTransform (const tf::Transform currentPose) const |
bool | getLastOdomPose (tf::Stamped< tf::Pose > &lastOdomPose) const |
bool | lookupLocalTransform (const std::string &targetFrame, const ros::Time &t, tf::StampedTransform &localTransform) const |
lookup the local target frame in the base frame (local transform) | |
bool | lookupOdomPose (const ros::Time &t, tf::Stamped< tf::Pose > &odomPose) const |
look up the odom pose at a certain time through tf | |
bool | lookupOdomTransform (const ros::Time &t, tf::Transform &odomTransform) const |
looks up the odometry pose at time t and then calls computeOdomTransform() | |
bool | lookupPoseHeight (const ros::Time &t, double &poseHeight) const |
lookup the height of the torso, based on tf between the base and footprint frames | |
MotionModel (ros::NodeHandle *nh, EngineT *rngEngine, tf::TransformListener *tf, const std::string &odomFrameId, const std::string &baseFrameId) | |
void | reset () |
void | storeOdomPose (const tf::Stamped< tf::Pose > &odomPose) |
store odomPose as m_lastOdomPose to compute future odom transforms | |
virtual | ~MotionModel () |
Protected Member Functions | |
void | applyOdomTransform (tf::Pose &particlePose, const tf::Transform &odomTransform) |
apply odomTransform to one particle pose (with random noise and calibration) | |
tf::Transform | calibrateOdometry (const tf::Transform &odomTransform) const |
tf::Transform | odomTransformNoise (const tf::Transform &odomTransform) |
void | transformPose (tf::Pose &particlePose, const tf::Transform &odomTransform) |
Transform a particle's pose with the relative odomTransform with added random noise. | |
Protected Attributes | |
std::string | m_baseFrameId |
bool | m_firstOdometryReceived |
std::string | m_footprintFrameId |
tf::Stamped< tf::Pose > | m_lastOdomPose |
Eigen::Matrix3d | m_odomCalibration2D |
systematic calibration parameters for odometry drift | |
std::string | m_odomFrameId |
Eigen::Matrix3d | m_odomNoise2D |
variance parameters for calibrated odometry noise | |
double | m_odomNoisePitch |
std.dev param of noise in pitch (depending on distance traveled) | |
double | m_odomNoiseRoll |
std.dev param of noise in roll (depending on distance traveled) | |
double | m_odomNoiseZ |
std.dev param of noise in Z (depending on distance traveled) | |
NormalGeneratorT | m_rngNormal |
UniformGeneratorT | m_rngUniform |
tf::TransformListener * | m_tfListener |
Definition at line 35 of file MotionModel.h.
humanoid_localization::MotionModel::MotionModel | ( | ros::NodeHandle * | nh, |
EngineT * | rngEngine, | ||
tf::TransformListener * | tf, | ||
const std::string & | odomFrameId, | ||
const std::string & | baseFrameId | ||
) |
Definition at line 31 of file MotionModel.cpp.
humanoid_localization::MotionModel::~MotionModel | ( | ) | [virtual] |
Definition at line 88 of file MotionModel.cpp.
void humanoid_localization::MotionModel::applyOdomTransform | ( | Particles & | particles, |
const tf::Transform & | odomTransform | ||
) |
apply odomTransform to all particles (with random noise and calibration)
Definition at line 128 of file MotionModel.cpp.
void humanoid_localization::MotionModel::applyOdomTransform | ( | tf::Pose & | particlePose, |
const tf::Transform & | odomTransform | ||
) | [protected] |
apply odomTransform to one particle pose (with random noise and calibration)
Definition at line 124 of file MotionModel.cpp.
bool humanoid_localization::MotionModel::applyOdomTransformTemporal | ( | Particles & | particles, |
const ros::Time & | t, | ||
double | dt | ||
) |
apply odomTransform to all particles (noisy), with temporal sampling over the range of dt. Times are sampled in an interval +-dt/2 around t, iff dt > 0.0.
Definition at line 136 of file MotionModel.cpp.
tf::Transform humanoid_localization::MotionModel::calibrateOdometry | ( | const tf::Transform & | odomTransform | ) | const [protected] |
Definition at line 178 of file MotionModel.cpp.
tf::Transform humanoid_localization::MotionModel::computeOdomTransform | ( | const tf::Transform | currentPose | ) | const |
computes the odometry transform from m_lastOdomPose to currentPose as relative 6D rigid body transform (=identity if m_lastOdomPose not available)
Definition at line 209 of file MotionModel.cpp.
bool humanoid_localization::MotionModel::getLastOdomPose | ( | tf::Stamped< tf::Pose > & | lastOdomPose | ) | const |
get the last stored odomPose returns false when there is no valid previous pose stored
Definition at line 264 of file MotionModel.cpp.
bool humanoid_localization::MotionModel::lookupLocalTransform | ( | const std::string & | targetFrame, |
const ros::Time & | t, | ||
tf::StampedTransform & | localTransform | ||
) | const |
lookup the local target frame in the base frame (local transform)
Definition at line 248 of file MotionModel.cpp.
bool humanoid_localization::MotionModel::lookupOdomPose | ( | const ros::Time & | t, |
tf::Stamped< tf::Pose > & | odomPose | ||
) | const |
look up the odom pose at a certain time through tf
Definition at line 229 of file MotionModel.cpp.
bool humanoid_localization::MotionModel::lookupOdomTransform | ( | const ros::Time & | t, |
tf::Transform & | odomTransform | ||
) | const |
looks up the odometry pose at time t and then calls computeOdomTransform()
Definition at line 194 of file MotionModel.cpp.
bool humanoid_localization::MotionModel::lookupPoseHeight | ( | const ros::Time & | t, |
double & | poseHeight | ||
) | const |
lookup the height of the torso, based on tf between the base and footprint frames
tf::Transform humanoid_localization::MotionModel::odomTransformNoise | ( | const tf::Transform & | odomTransform | ) | [protected] |
Generates motion noise corresponding to odomTransform May not be called in parallel, accesses the random generator m_rngNormal
Definition at line 93 of file MotionModel.cpp.
Definition at line 120 of file MotionModel.cpp.
void humanoid_localization::MotionModel::storeOdomPose | ( | const tf::Stamped< tf::Pose > & | odomPose | ) |
store odomPose as m_lastOdomPose to compute future odom transforms
Definition at line 218 of file MotionModel.cpp.
void humanoid_localization::MotionModel::transformPose | ( | tf::Pose & | particlePose, |
const tf::Transform & | odomTransform | ||
) | [protected] |
Transform a particle's pose with the relative odomTransform with added random noise.
std::string humanoid_localization::MotionModel::m_baseFrameId [protected] |
Definition at line 106 of file MotionModel.h.
Definition at line 109 of file MotionModel.h.
std::string humanoid_localization::MotionModel::m_footprintFrameId [protected] |
Definition at line 107 of file MotionModel.h.
Definition at line 110 of file MotionModel.h.
Eigen::Matrix3d humanoid_localization::MotionModel::m_odomCalibration2D [protected] |
systematic calibration parameters for odometry drift
Definition at line 96 of file MotionModel.h.
std::string humanoid_localization::MotionModel::m_odomFrameId [protected] |
Definition at line 105 of file MotionModel.h.
Eigen::Matrix3d humanoid_localization::MotionModel::m_odomNoise2D [protected] |
variance parameters for calibrated odometry noise
Definition at line 94 of file MotionModel.h.
double humanoid_localization::MotionModel::m_odomNoisePitch [protected] |
std.dev param of noise in pitch (depending on distance traveled)
Definition at line 102 of file MotionModel.h.
double humanoid_localization::MotionModel::m_odomNoiseRoll [protected] |
std.dev param of noise in roll (depending on distance traveled)
Definition at line 100 of file MotionModel.h.
double humanoid_localization::MotionModel::m_odomNoiseZ [protected] |
std.dev param of noise in Z (depending on distance traveled)
Definition at line 98 of file MotionModel.h.
Definition at line 90 of file MotionModel.h.
Definition at line 91 of file MotionModel.h.
Definition at line 88 of file MotionModel.h.