#include <RevoluteJointFilter.h>

Inheritance diagram for omip::RevoluteJointFilter:

Public Member Functions
RevoluteJointFilterPtr	clone () const
virtual void	correctState ()
virtual void	estimateMeasurementHistoryLikelihood ()
virtual void	estimateUnnormalizedModelProbability ()
virtual JointFilterType	getJointFilterType () const
virtual std::string	getJointFilterTypeStr () const
virtual std::vector < visualization_msgs::Marker >	getJointMarkersInRRBFrame () const
virtual geometry_msgs::TwistWithCovariance	getPredictedSRBDeltaPoseWithCovInSensorFrame ()
	Generate a prediction about the change in pose of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF)
virtual geometry_msgs::TwistWithCovariance	getPredictedSRBPoseWithCovInSensorFrame ()
	We use the kinematic structure to predict the next pose-twist of the second rigid body of the joint (with covariance based on the model uncertainty) in sensor frame (the frame where we track the RBs). This values will be passed to the MultiRBTracker.
virtual geometry_msgs::TwistWithCovariance	getPredictedSRBVelocityWithCovInSensorFrame ()
	Generate a prediction about the velocity of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF)
virtual void	initialize ()
virtual void	predictMeasurement ()
virtual void	predictState (double time_interval_ns)
	First step when updating the filter. The next state is predicted from current state and system model.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	RevoluteJointFilter ()
	RevoluteJointFilter (const RevoluteJointFilter &rev_joint)
virtual void	setCovarianceDeltaMeasurementAngular (double sigma_delta_meas_uncertainty_angular)
virtual void	setMaxRadiusDistanceRevolute (const double &value)
virtual void	setMinRotationRevolute (const double &value)
virtual	~RevoluteJointFilter ()
Protected Member Functions
void	_initializeEKF ()
void	_initializeMeasurementModel ()
void	_initializeSystemModel ()
virtual RevoluteJointFilter *	doClone () const
Protected Attributes
double	_accumulated_rotation
BFL::ExtendedKalmanFilter *	_ekf
BFL::AnalyticMeasurementModelGaussianUncertainty *	_meas_MODEL
BFL::NonLinearRevoluteMeasurementPdf *	_meas_PDF
double	_rev_max_joint_distance_for_ee
double	_rev_min_rot_for_ee
double	_sigma_delta_meas_uncertainty_angular
BFL::LinearAnalyticSystemModelGaussianUncertainty *	_sys_MODEL
BFL::LinearAnalyticConditionalGaussian *	_sys_PDF

Detailed Description

Definition at line 54 of file RevoluteJointFilter.h.

Constructor & Destructor Documentation

RevoluteJointFilter::RevoluteJointFilter ( )

Constructor

Definition at line 45 of file RevoluteJointFilter.cpp.

RevoluteJointFilter::~RevoluteJointFilter ( ) [virtual]

Destructor

Definition at line 185 of file RevoluteJointFilter.cpp.

RevoluteJointFilter::RevoluteJointFilter ( const RevoluteJointFilter & rev_joint )

Copy constructor

Definition at line 214 of file RevoluteJointFilter.cpp.

Member Function Documentation

void RevoluteJointFilter::_initializeEKF ( ) [protected]

Definition at line 151 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::_initializeMeasurementModel ( ) [protected]

Definition at line 135 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::_initializeSystemModel ( ) [protected]

Definition at line 105 of file RevoluteJointFilter.cpp.

RevoluteJointFilterPtr omip::RevoluteJointFilter::clone ( ) const [inline]

Creates a new Joint object with the same values as this one and passes its reference

Returns:: - A reference to the clone of this Feature object

Reimplemented from omip::JointFilter.

Definition at line 76 of file RevoluteJointFilter.h.

void RevoluteJointFilter::correctState ( ) [virtual]

Use the observed pose as measurement for the EKF -> Update joint parameters

Reimplemented from omip::JointFilter.

Definition at line 277 of file RevoluteJointFilter.cpp.

virtual RevoluteJointFilter* omip::RevoluteJointFilter::doClone ( ) const [inline, protected, virtual]

Implements omip::JointFilter.

Definition at line 183 of file RevoluteJointFilter.h.

void RevoluteJointFilter::estimateMeasurementHistoryLikelihood ( ) [virtual]

Measure the error of the last joint parameters by testing them against the full observed trajectory of the second RB relative to the reference RB

Reimplemented from omip::JointFilter.

Definition at line 366 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::estimateUnnormalizedModelProbability ( ) [virtual]

Reimplemented from omip::JointFilter.

Definition at line 464 of file RevoluteJointFilter.cpp.

JointFilterType RevoluteJointFilter::getJointFilterType ( ) const [virtual]

Return the joint type as one of the possible JointFilterTypes

Implements omip::JointFilter.

Definition at line 876 of file RevoluteJointFilter.cpp.

std::string RevoluteJointFilter::getJointFilterTypeStr ( ) const [virtual]

Return the joint type as an string

Implements omip::JointFilter.

Definition at line 881 of file RevoluteJointFilter.cpp.

std::vector< visualization_msgs::Marker > RevoluteJointFilter::getJointMarkersInRRBFrame ( ) const [virtual]

Return rviz markers that show the type and parameters of the estimated joint

Implements omip::JointFilter.

Definition at line 628 of file RevoluteJointFilter.cpp.

geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame ( ) [virtual]

Generate a prediction about the change in pose of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF)

Returns:: geometry_msgs::TwistWithCovariance Change in pose of the second rigid body in form of a twist with covariance based on the model uncertainty

Implements omip::JointFilter.

Definition at line 496 of file RevoluteJointFilter.cpp.

geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBPoseWithCovInSensorFrame ( ) [virtual]

We use the kinematic structure to predict the next pose-twist of the second rigid body of the joint (with covariance based on the model uncertainty) in sensor frame (the frame where we track the RBs). This values will be passed to the MultiRBTracker.

Returns:: geometry_msgs::TwistWithCovariance Pose of the second rigid body in form of a twist with covariance based on the model uncertainty

Implements omip::JointFilter.

Definition at line 556 of file RevoluteJointFilter.cpp.

geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBVelocityWithCovInSensorFrame ( ) [virtual]

Generate a prediction about the velocity of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF)

Returns:: geometry_msgs::TwistWithCovariance Velocity of the second rigid body in form of a twist with covariance based on the model uncertainty

Implements omip::JointFilter.

Definition at line 526 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::initialize ( ) [virtual]

Reimplemented from omip::JointFilter.

Definition at line 62 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::predictMeasurement ( ) [virtual]

Generate a hypothesis about the pose of the second rigid body based on the pose of the reference rigid body and the internal state (joint parameters and latent variable)

Reimplemented from omip::JointFilter.

Definition at line 254 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::predictState ( double time_interval_ns ) [virtual]

First step when updating the filter. The next state is predicted from current state and system model.

Reimplemented from omip::JointFilter.

Definition at line 225 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::setCovarianceDeltaMeasurementAngular ( double sigma_delta_meas_uncertainty_angular ) [virtual]

Definition at line 57 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::setMaxRadiusDistanceRevolute ( const double & value ) [virtual]

Definition at line 100 of file RevoluteJointFilter.cpp.

void RevoluteJointFilter::setMinRotationRevolute ( const double & value ) [virtual]

Definition at line 95 of file RevoluteJointFilter.cpp.

Member Data Documentation

double omip::RevoluteJointFilter::_accumulated_rotation [protected]

Definition at line 188 of file RevoluteJointFilter.h.

BFL::ExtendedKalmanFilter* omip::RevoluteJointFilter::_ekf [protected]

Definition at line 177 of file RevoluteJointFilter.h.

BFL::AnalyticMeasurementModelGaussianUncertainty* omip::RevoluteJointFilter::_meas_MODEL [protected]

Definition at line 175 of file RevoluteJointFilter.h.

BFL::NonLinearRevoluteMeasurementPdf* omip::RevoluteJointFilter::_meas_PDF [protected]

Definition at line 174 of file RevoluteJointFilter.h.

double omip::RevoluteJointFilter::_rev_max_joint_distance_for_ee [protected]

Definition at line 169 of file RevoluteJointFilter.h.

double omip::RevoluteJointFilter::_rev_min_rot_for_ee [protected]

Definition at line 168 of file RevoluteJointFilter.h.

double omip::RevoluteJointFilter::_sigma_delta_meas_uncertainty_angular [protected]

A Revolute Joint constrains 5 of the 6 dofs of the relative motion between RBs Revolute Joints have joint orientation and joint position as motion parameters The latent variable is the angle of rotation of the second RB wrt to the reference RB

Definition at line 166 of file RevoluteJointFilter.h.

BFL::LinearAnalyticSystemModelGaussianUncertainty* omip::RevoluteJointFilter::_sys_MODEL [protected]

Definition at line 172 of file RevoluteJointFilter.h.

BFL::LinearAnalyticConditionalGaussian* omip::RevoluteJointFilter::_sys_PDF [protected]

Definition at line 171 of file RevoluteJointFilter.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation