#include <PrismaticJointFilter.h>

Inheritance diagram for omip::PrismaticJointFilter:

Public Member Functions
PrismaticJointFilterPtr	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	PrismaticJointFilter ()
	PrismaticJointFilter (const PrismaticJointFilter &prismatic_joint)
virtual void	setCovarianceDeltaMeasurementLinear (double delta_meas)
virtual	~PrismaticJointFilter ()
Protected Member Functions
void	_initializeEKF ()
void	_initializeMeasurementModel ()
void	_initializeSystemModel ()
virtual PrismaticJointFilter *	doClone () const
Protected Attributes
BFL::ExtendedKalmanFilter *	_ekf
BFL::AnalyticMeasurementModelGaussianUncertainty *	_meas_MODEL
BFL::NonLinearPrismaticMeasurementPdf *	_meas_PDF
double	_sigma_delta_meas_uncertainty_linear
BFL::LinearAnalyticSystemModelGaussianUncertainty *	_sys_MODEL
BFL::LinearAnalyticConditionalGaussian *	_sys_PDF

Detailed Description

Definition at line 54 of file PrismaticJointFilter.h.

Constructor & Destructor Documentation

PrismaticJointFilter::PrismaticJointFilter ( )

Constructor

EKF internal state:

x(1) = PrismJointOrientation_phi x(2) = PrismJointOrientation_theta PrismJointOrientation is represented in spherical coords NOTE: I try to keep theta between 0 and pi and phi between 0 and two pi x(4) = PrismJointVariable x(5) = PrismJointVariable_d

EKF measurement:

m(1) = TwistLinearPart_x m(2) = TwistLinearPart_y m(3) = TwistLinearPart_z m(4) = TwistAngularPart_x m(5) = TwistAngularPart_y m(6) = TwistAngularPart_z

Definition at line 43 of file PrismaticJointFilter.cpp.

PrismaticJointFilter::~PrismaticJointFilter ( ) [virtual]

Destructor

Definition at line 190 of file PrismaticJointFilter.cpp.

PrismaticJointFilter::PrismaticJointFilter ( const PrismaticJointFilter & prismatic_joint )

Copy constructor

Definition at line 219 of file PrismaticJointFilter.cpp.

Member Function Documentation

void PrismaticJointFilter::_initializeEKF ( ) [protected]

Definition at line 157 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::_initializeMeasurementModel ( ) [protected]

Definition at line 126 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::_initializeSystemModel ( ) [protected]

Definition at line 99 of file PrismaticJointFilter.cpp.

PrismaticJointFilterPtr omip::PrismaticJointFilter::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 PrismaticJointFilter.h.

void PrismaticJointFilter::correctState ( ) [virtual]

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

Reimplemented from omip::JointFilter.

Definition at line 336 of file PrismaticJointFilter.cpp.

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

Implements omip::JointFilter.

Definition at line 175 of file PrismaticJointFilter.h.

void PrismaticJointFilter::estimateMeasurementHistoryLikelihood ( ) [virtual]

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

Reimplemented from omip::JointFilter.

Definition at line 413 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::estimateUnnormalizedModelProbability ( ) [virtual]

Reimplemented from omip::JointFilter.

Definition at line 498 of file PrismaticJointFilter.cpp.

JointFilterType PrismaticJointFilter::getJointFilterType ( ) const [virtual]

Return the joint type as one of the possible JointFilterTypes

Implements omip::JointFilter.

Definition at line 796 of file PrismaticJointFilter.cpp.

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

Return the joint type as an string

Implements omip::JointFilter.

Definition at line 801 of file PrismaticJointFilter.cpp.

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

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

Implements omip::JointFilter.

Definition at line 574 of file PrismaticJointFilter.cpp.

geometry_msgs::TwistWithCovariance PrismaticJointFilter::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 255 of file PrismaticJointFilter.cpp.

geometry_msgs::TwistWithCovariance PrismaticJointFilter::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 503 of file PrismaticJointFilter.cpp.

geometry_msgs::TwistWithCovariance PrismaticJointFilter::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 285 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::initialize ( ) [virtual]

Reimplemented from omip::JointFilter.

Definition at line 60 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::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 315 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::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 230 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::setCovarianceDeltaMeasurementLinear ( double delta_meas ) [virtual]

Definition at line 55 of file PrismaticJointFilter.cpp.

Member Data Documentation

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

Definition at line 169 of file PrismaticJointFilter.h.

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

Definition at line 167 of file PrismaticJointFilter.h.

BFL::NonLinearPrismaticMeasurementPdf* omip::PrismaticJointFilter::_meas_PDF [protected]

Definition at line 166 of file PrismaticJointFilter.h.

double omip::PrismaticJointFilter::_sigma_delta_meas_uncertainty_linear [protected]

A Prismatic Joint constrains 5 of the 6 dofs of the relative motion between RBs Prismatic Joints have only joint orientation as motion parameters The latent variable is the distance of the translation of the second RB wrt to the reference RB

Definition at line 161 of file PrismaticJointFilter.h.

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

Definition at line 164 of file PrismaticJointFilter.h.

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

Definition at line 163 of file PrismaticJointFilter.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