#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) More...

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. More...

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) More...

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. More...

EIGEN_MAKE_ALIGNED_OPERATOR_NEW	PrismaticJointFilter ()

	PrismaticJointFilter (const PrismaticJointFilter &prismatic_joint)

virtual void	setCovarianceDeltaMeasurementLinear (double delta_meas)

virtual	~PrismaticJointFilter ()

Public Member Functions inherited from omip::JointFilter
JointFilterPtr	clone () const
	Creates a new Joint object with the same values as this one and passes its reference. More...

virtual double	getCovarianceOrientationPhiPhiInRRBFrame () const

virtual double	getCovarianceOrientationPhiThetaInRRBFrame () const

virtual double	getCovarianceOrientationThetaThetaInRRBFrame () const

virtual JointCombinedFilterId	getJointId () const
	Get the unique Id that identifies this joint. More...

virtual Eigen::Vector3d	getJointOrientationRPYInRRBFrame () const

virtual Eigen::Vector3d	getJointOrientationUnitaryVector () const

virtual Eigen::Vector3d	getJointPositionInRRBFrame () const

virtual double	getJointState () const
	Return the joint state. Useful for plotting and testing purposes (monitoring action) More...

virtual double	getJointVelocity () const
	Return the joint velocity. Useful for plotting and testing purposes (monitoring action) More...

virtual double	getLikelihoodOfLastMeasurements () const
	Return the estimated error of the last joint parameters by testing them against the full observed trajectory. More...

virtual double	getLoopPeriodNS () const

virtual double	getModelPriorProbability () const
	Get the prior probability of this type of model/joint. We could use this in the feature if the robot is in environments where is more probable to encounter joints of certain type. More...

virtual double	getNormalizingTerm ()

virtual int	getNumSamplesForLikelihoodEstimation () const

virtual double	getOrientationPhiInRRBFrame () const

virtual double	getOrientationThetaInRRBFrame () const

virtual Eigen::Twistd	getPredictedMeasurement () const
	Get the predicted pose-twist of the second rigid body (SRB) using the frame of the reference rigid body (RRBF) as observation and ref frame. More...

virtual double	getProbabilityOfJointFilter () const
	Return the normalized joint filter probability as p(Model\|Data) = p(Data\|Model) x p(Model) / p(Data) where SUM_all_models( p(Model\|Data) ) = 1. More...

virtual double	getUnnormalizedProbabilityOfJointFilter () const
	Return the unnormalized joint filter probability as p(Data\|Model) x p(Model) More...

	JointFilter ()
	Constructor. More...

	JointFilter (const JointFilter &joint)
	Copy constructor. More...

virtual void	setCovarianceAdditiveSystemNoiseJointState (double sigma_sys_noise_pv)

virtual void	setCovarianceAdditiveSystemNoiseJointVelocity (double sigma_sys_noise_pvd)

virtual void	setCovarianceAdditiveSystemNoisePhi (double sigma_sys_noise_phi)

virtual void	setCovarianceAdditiveSystemNoisePx (double sigma_sys_noise_px)

virtual void	setCovarianceAdditiveSystemNoisePy (double sigma_sys_noise_py)

virtual void	setCovarianceAdditiveSystemNoisePz (double sigma_sys_noise_pz)

virtual void	setCovarianceAdditiveSystemNoiseTheta (double sigma_sys_noise_theta)

virtual void	setCovarianceMeasurementNoise (double sigma_meas_noise)

virtual void	setCovariancePrior (double prior_cov_vel)

virtual void	setInitialMeasurement (const joint_measurement_t &initial_measurement, const Eigen::Twistd &rrb_pose_at_srb_birth_in_sf, const Eigen::Twistd &srb_pose_at_srb_birth_in_sf)

virtual void	setJointId (JointCombinedFilterId joint_id)

virtual void	setLikelihoodOfLastMeasurements (double likelihood)
	Set the estimated error of the last joint parameters (only useful for the disconnected joint because we do not estimate the likelihood internally) More...

virtual void	setLoopPeriodNS (double loop_period_ns)

virtual void	setMeasurement (joint_measurement_t acquired_measurement, const double &measurement_timestamp_ns)
	Set the latest acquired measurement. More...

virtual void	setModelPriorProbability (double model_prior_probability)
	Set the prior probability of this type of model/joint. We could use this in the feature if the robot is in environments where is more probable to encounter joints of certain type. More...

virtual void	setNormalizingTerm (double normalizing_term)
	Set the normalizing term from the combined filter as the sum of all unnormalized probabilities. More...

virtual void	setNumSamplesForLikelihoodEstimation (int likelihood_sample_num)

virtual	~JointFilter ()
	Destructor. More...

Protected Member Functions
void	_initializeEKF ()

void	_initializeMeasurementModel ()

void	_initializeSystemModel ()

virtual PrismaticJointFilter *	doClone () const

Protected Member Functions inherited from omip::JointFilter
void	_initVariables (JointCombinedFilterId joint_id)
	Initialize all variables of the filter. More...

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

Protected Attributes inherited from omip::JointFilter
Eigen::Matrix< double, 6, 6 >	_current_delta_pose_cov_in_rrbf

Eigen::Twistd	_current_delta_pose_in_rrbf

std::vector< Eigen::Twistd >	_delta_poses_in_rrbf

bool	_externally_set_likelihood

bool	_from_inverted_to_non_inverted

bool	_from_non_inverted_to_inverted

bool	_inverted_delta_srb_pose_in_rrbf

double	_joint_acceleration

JointCombinedFilterId	_joint_id

Eigen::Vector3d	_joint_orientation

double	_joint_orientation_phi

double	_joint_orientation_theta

Eigen::Vector3d	_joint_position

double	_joint_state

std::vector< double >	_joint_states_all

double	_joint_velocity

int	_likelihood_sample_num

double	_loop_period_ns

double	_measurement_timestamp_ns

double	_measurements_likelihood

double	_model_prior_probability

double	_normalizing_term

Eigen::Twistd	_predicted_delta_pose_in_rrbf

Eigen::Twistd	_previous_delta_pose_in_rrbf

double	_prior_cov_vel

Eigen::Vector3d	_rrb_centroid_in_sf

Eigen::Twistd	_rrb_current_pose_in_sf

Eigen::Twistd	_rrb_current_vel_in_sf

int	_rrb_id

Eigen::Matrix< double, 6, 6 >	_rrb_pose_cov_in_sf

Eigen::Twistd	_rrb_previous_pose_in_sf

Eigen::Matrix< double, 6, 6 >	_rrb_vel_cov_in_sf

double	_sigma_meas_noise

double	_sigma_sys_noise_jv

double	_sigma_sys_noise_jvd

double	_sigma_sys_noise_phi

double	_sigma_sys_noise_px

double	_sigma_sys_noise_py

double	_sigma_sys_noise_pz

double	_sigma_sys_noise_theta

Eigen::Vector3d	_srb_centroid_in_sf

Eigen::Matrix< double, 6, 6 >	_srb_current_pose_cov_in_rrbf

Eigen::Twistd	_srb_current_pose_in_rrbf

Eigen::Twistd	_srb_current_pose_in_sf

Eigen::Twistd	_srb_current_vel_in_sf

Eigen::Matrix< double, 6, 6 >	_srb_initial_pose_cov_in_rrbf

Eigen::Twistd	_srb_initial_pose_in_rrbf

Eigen::Matrix< double, 6, 6 >	_srb_pose_cov_in_sf

Eigen::Twistd	_srb_predicted_pose_in_rrbf

Eigen::Twistd	_srb_previous_pose_in_rrbf

Eigen::Twistd	_srb_previous_predicted_pose_in_rrbf

tf::TransformBroadcaster	_tf_pub

double	_uncertainty_joint_acceleration

Eigen::Matrix2d	_uncertainty_joint_orientation_phitheta

Eigen::Matrix3d	_uncertainty_joint_position

double	_uncertainty_joint_state

double	_uncertainty_joint_velocity

double	_unnormalized_model_probability

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

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 332 of file PrismaticJointFilter.cpp.

virtual PrismaticJointFilter* omip::PrismaticJointFilter::doClone ( ) const

inlineprotectedvirtual

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 409 of file PrismaticJointFilter.cpp.

void PrismaticJointFilter::estimateUnnormalizedModelProbability ( )

virtual

Reimplemented from omip::JointFilter.

Definition at line 494 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 791 of file PrismaticJointFilter.cpp.

std::string PrismaticJointFilter::getJointFilterTypeStr ( ) const

virtual

Return the joint type as an string

Implements omip::JointFilter.

Definition at line 796 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 569 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 499 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 283 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 311 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