#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

Detailed Description

Definition at line 35 of file MotionModel.h.

Constructor & Destructor Documentation

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.

Member Function Documentation

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]

Returns:: calibrated odometry transform w.r.t. 2D drift (pos. + orientation)

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