#include <JointFilter.h>

Inheritance diagram for omip::JointFilter:

Public Member Functions
JointFilterPtr	clone () const
	Creates a new Joint object with the same values as this one and passes its reference.
virtual void	correctState ()
	Correct the state of the filter (the joint parameters) based on the difference between the predicted and the last acquired measurement It is only implemented for revolute and prismatic because rigid and disconnected have no parameters to correct.
virtual void	estimateMeasurementHistoryLikelihood ()
	Estimate the error of the last joint parameters by testing them against the full observed trajectory of the second RB relative to the reference RB -> Obtain the likelihood of the measurements given the model and the model parameters Implemented for prismatic, revolute and rigid joints, but not for disconnected (there is no way to estimate its likelihood, we use a value that is a threshold)
virtual void	estimateUnnormalizedModelProbability ()
virtual double	getCovarianceOrientationPhiPhiInRRBFrame () const
virtual double	getCovarianceOrientationPhiThetaInRRBFrame () const
virtual double	getCovarianceOrientationThetaThetaInRRBFrame () const
virtual JointFilterType	getJointFilterType () const =0
	Get the joint type as one of the possible JointFilterTypes.
virtual std::string	getJointFilterTypeStr () const =0
	Get the joint type as a string (printing purposes)
virtual JointCombinedFilterId	getJointId () const
	Get the unique Id that identifies this joint.
virtual std::vector < visualization_msgs::Marker >	getJointMarkersInRRBFrame () const =0
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)
virtual double	getJointVelocity () const
	Return the joint velocity. Useful for plotting and testing purposes (monitoring action)
virtual double	getLikelihoodOfLastMeasurements () const
	Return the estimated error of the last joint parameters by testing them against the full observed trajectory.
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.
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.
virtual geometry_msgs::TwistWithCovariance	getPredictedSRBDeltaPoseWithCovInSensorFrame ()=0
	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 ()=0
	Generate a prediction about the pose-twist 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	getPredictedSRBVelocityWithCovInSensorFrame ()=0
	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 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.
virtual double	getUnnormalizedProbabilityOfJointFilter () const
	Return the unnormalized joint filter probability as p(Data\|Model) x p(Model)
virtual void	initialize ()
	JointFilter ()
	Constructor.
	JointFilter (const JointFilter &joint)
	Copy constructor.
virtual void	predictMeasurement ()
	Generate a prediction about the pose-twist of the second rigid body (SRB) using the frame of the reference rigid body (RRBF) as observation and ref frame based on the belief state (joint parameters and latent variable) and the initial pose of the SRB in the RRBF Implemented for prismatic, revolute and rigid joints, but not for disconnected (there is no way to predict the next measurement because there is no constraint in the motion if the joint is disconnected)
virtual void	predictState (double time_interval_ns)
	Predict the next state.
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)
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.
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.
virtual void	setNormalizingTerm (double normalizing_term)
	Set the normalizing term from the combined filter as the sum of all unnormalized probabilities.
virtual void	setNumSamplesForLikelihoodEstimation (int likelihood_sample_num)
virtual	~JointFilter ()
	Destructor.
Protected Member Functions
void	_initVariables (JointCombinedFilterId joint_id)
	Initialize all variables of the filter.
virtual JointFilter *	doClone () const =0
Protected Attributes
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 82 of file JointFilter.h.

Constructor & Destructor Documentation

JointFilter::JointFilter ( )

Constructor.

Definition at line 9 of file JointFilter.cpp.

JointFilter::~JointFilter ( ) [virtual]

Destructor.

Definition at line 145 of file JointFilter.cpp.

JointFilter::JointFilter ( const JointFilter & joint )

Copy constructor.

Definition at line 150 of file JointFilter.cpp.

Member Function Documentation

void JointFilter::_initVariables ( JointCombinedFilterId joint_id ) [protected]

Initialize all variables of the filter.

Parameters:

joint_id Id of the joint

Definition at line 106 of file JointFilter.cpp.

JointFilterPtr omip::JointFilter::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 in omip::PrismaticJointFilter, omip::RevoluteJointFilter, omip::DisconnectedJointFilter, and omip::RigidJointFilter.

Definition at line 102 of file JointFilter.h.

virtual void omip::JointFilter::correctState ( ) [inline, virtual]

Correct the state of the filter (the joint parameters) based on the difference between the predicted and the last acquired measurement It is only implemented for revolute and prismatic because rigid and disconnected have no parameters to correct.

Reimplemented in omip::PrismaticJointFilter, and omip::RevoluteJointFilter.

Definition at line 134 of file JointFilter.h.

virtual JointFilter* omip::JointFilter::doClone ( ) const [protected, pure virtual]

Implemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

virtual void omip::JointFilter::estimateMeasurementHistoryLikelihood ( ) [inline, virtual]

Estimate the error of the last joint parameters by testing them against the full observed trajectory of the second RB relative to the reference RB -> Obtain the likelihood of the measurements given the model and the model parameters Implemented for prismatic, revolute and rigid joints, but not for disconnected (there is no way to estimate its likelihood, we use a value that is a threshold)

Reimplemented in omip::PrismaticJointFilter, omip::RevoluteJointFilter, and omip::RigidJointFilter.

Definition at line 159 of file JointFilter.h.

virtual void omip::JointFilter::estimateUnnormalizedModelProbability ( ) [inline, virtual]

Reimplemented in omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::RevoluteJointFilter.

Definition at line 161 of file JointFilter.h.

double JointFilter::getCovarianceOrientationPhiPhiInRRBFrame ( ) const [virtual]

Definition at line 385 of file JointFilter.cpp.

double JointFilter::getCovarianceOrientationPhiThetaInRRBFrame ( ) const [virtual]

Definition at line 395 of file JointFilter.cpp.

double JointFilter::getCovarianceOrientationThetaThetaInRRBFrame ( ) const [virtual]

Definition at line 390 of file JointFilter.cpp.

virtual JointFilterType omip::JointFilter::getJointFilterType ( ) const [pure virtual]

Get the joint type as one of the possible JointFilterTypes.

Returns:: JointFilterType The type of this joint filter

Implemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

virtual std::string omip::JointFilter::getJointFilterTypeStr ( ) const [pure virtual]

Get the joint type as a string (printing purposes)

Returns:: std::string The type of this joint filter as a string

Implemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

JointCombinedFilterId JointFilter::getJointId ( ) const [virtual]

Get the unique Id that identifies this joint.

Returns:: longint Joint id of this joint

Definition at line 360 of file JointFilter.cpp.

virtual std::vector<visualization_msgs::Marker> omip::JointFilter::getJointMarkersInRRBFrame ( ) const [pure virtual]

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

Implemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

Eigen::Vector3d JointFilter::getJointOrientationRPYInRRBFrame ( ) const [virtual]

Definition at line 405 of file JointFilter.cpp.

Eigen::Vector3d JointFilter::getJointOrientationUnitaryVector ( ) const [virtual]

Definition at line 414 of file JointFilter.cpp.

Eigen::Vector3d JointFilter::getJointPositionInRRBFrame ( ) const [virtual]

Definition at line 400 of file JointFilter.cpp.

double JointFilter::getJointState ( ) const [virtual]

Return the joint state. Useful for plotting and testing purposes (monitoring action)

Returns:: double Currently estimated joint state

Definition at line 365 of file JointFilter.cpp.

double JointFilter::getJointVelocity ( ) const [virtual]

Return the joint velocity. Useful for plotting and testing purposes (monitoring action)

Returns:: double Currently estimated joint velocity

Definition at line 370 of file JointFilter.cpp.

double JointFilter::getLikelihoodOfLastMeasurements ( ) const [virtual]

Return the estimated error of the last joint parameters by testing them against the full observed trajectory.

Returns:: double Likelihood of the last relative trajectory of the SRB wrt the RRB given the model and the model parameters

Definition at line 334 of file JointFilter.cpp.

double JointFilter::getLoopPeriodNS ( ) const [virtual]

Definition at line 299 of file JointFilter.cpp.

double JointFilter::getModelPriorProbability ( ) const [virtual]

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.

Returns:: double The prior probability of this type of model/joint

Definition at line 329 of file JointFilter.cpp.

double JointFilter::getNormalizingTerm ( ) [virtual]

Definition at line 319 of file JointFilter.cpp.

int JointFilter::getNumSamplesForLikelihoodEstimation ( ) const [virtual]

Definition at line 309 of file JointFilter.cpp.

double JointFilter::getOrientationPhiInRRBFrame ( ) const [virtual]

Definition at line 375 of file JointFilter.cpp.

double JointFilter::getOrientationThetaInRRBFrame ( ) const [virtual]

Definition at line 380 of file JointFilter.cpp.

Eigen::Twistd JointFilter::getPredictedMeasurement ( ) const [virtual]

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.

Returns:: Eigen::Twistd Predicted pose-twist of the second rigid body (SRB) using the frame of the reference rigid body (RRBF) as observation and ref frame

Definition at line 289 of file JointFilter.cpp.

virtual geometry_msgs::TwistWithCovariance omip::JointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame ( ) [pure 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

Implemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

virtual geometry_msgs::TwistWithCovariance omip::JointFilter::getPredictedSRBPoseWithCovInSensorFrame ( ) [pure virtual]

Generate a prediction about the pose-twist 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)

Implemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

virtual geometry_msgs::TwistWithCovariance omip::JointFilter::getPredictedSRBVelocityWithCovInSensorFrame ( ) [pure 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

Implemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

double JointFilter::getProbabilityOfJointFilter ( ) const [virtual]

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.

Returns:: double Normalized probability of this joint filter

Reimplemented in omip::DisconnectedJointFilter.

Definition at line 350 of file JointFilter.cpp.

double JointFilter::getUnnormalizedProbabilityOfJointFilter ( ) const [virtual]

Return the unnormalized joint filter probability as p(Data|Model) x p(Model)

Returns:: double Unnormalized probability of this joint filter

Definition at line 345 of file JointFilter.cpp.

void JointFilter::initialize ( ) [virtual]

Reimplemented in omip::RevoluteJointFilter, omip::PrismaticJointFilter, omip::RigidJointFilter, and omip::DisconnectedJointFilter.

Definition at line 464 of file JointFilter.cpp.

virtual void omip::JointFilter::predictMeasurement ( ) [inline, virtual]

Generate a prediction about the pose-twist of the second rigid body (SRB) using the frame of the reference rigid body (RRBF) as observation and ref frame based on the belief state (joint parameters and latent variable) and the initial pose of the SRB in the RRBF Implemented for prismatic, revolute and rigid joints, but not for disconnected (there is no way to predict the next measurement because there is no constraint in the motion if the joint is disconnected)

Reimplemented in omip::PrismaticJointFilter, omip::RevoluteJointFilter, and omip::RigidJointFilter.

Definition at line 127 of file JointFilter.h.

virtual void omip::JointFilter::predictState ( double time_interval_ns ) [inline, virtual]

Predict the next state.

Reimplemented in omip::PrismaticJointFilter, and omip::RevoluteJointFilter.

Definition at line 118 of file JointFilter.h.

void JointFilter::setCovarianceAdditiveSystemNoiseJointState ( double sigma_sys_noise_pv ) [virtual]

Definition at line 449 of file JointFilter.cpp.

void JointFilter::setCovarianceAdditiveSystemNoiseJointVelocity ( double sigma_sys_noise_pvd ) [virtual]

Definition at line 454 of file JointFilter.cpp.

void JointFilter::setCovarianceAdditiveSystemNoisePhi ( double sigma_sys_noise_phi ) [virtual]

Definition at line 424 of file JointFilter.cpp.

void JointFilter::setCovarianceAdditiveSystemNoisePx ( double sigma_sys_noise_px ) [virtual]

Definition at line 434 of file JointFilter.cpp.

void JointFilter::setCovarianceAdditiveSystemNoisePy ( double sigma_sys_noise_py ) [virtual]

Definition at line 439 of file JointFilter.cpp.

void JointFilter::setCovarianceAdditiveSystemNoisePz ( double sigma_sys_noise_pz ) [virtual]

Definition at line 444 of file JointFilter.cpp.

void JointFilter::setCovarianceAdditiveSystemNoiseTheta ( double sigma_sys_noise_theta ) [virtual]

Definition at line 429 of file JointFilter.cpp.

void JointFilter::setCovarianceMeasurementNoise ( double sigma_meas_noise ) [virtual]

Definition at line 459 of file JointFilter.cpp.

void JointFilter::setCovariancePrior ( double prior_cov_vel ) [virtual]

Definition at line 419 of file JointFilter.cpp.

void JointFilter::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]`

Definition at line 24 of file JointFilter.cpp.

void JointFilter::setJointId ( JointCombinedFilterId joint_id ) [virtual]

Definition at line 355 of file JointFilter.cpp.

void JointFilter::setLikelihoodOfLastMeasurements ( double likelihood ) [virtual]

Set the estimated error of the last joint parameters (only useful for the disconnected joint because we do not estimate the likelihood internally)

Parameters:

likelihood Likelihood of the last relative trajectory of the SRB wrt the RRB given the model and the model parameters

Definition at line 339 of file JointFilter.cpp.

void JointFilter::setLoopPeriodNS ( double loop_period_ns ) [virtual]

Definition at line 294 of file JointFilter.cpp.

void JointFilter::setMeasurement	(	joint_measurement_t	acquired_measurement,
		const double &	measurement_timestamp_ns
	)		`[virtual]`

Set the latest acquired measurement.

Parameters:

acquired_measurement Latest acquired measurement

Definition at line 189 of file JointFilter.cpp.

void JointFilter::setModelPriorProbability ( double model_prior_probability ) [virtual]

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.

Parameters:

model_prior_probability Prior probability of this type of model/joint

Definition at line 324 of file JointFilter.cpp.

void JointFilter::setNormalizingTerm ( double normalizing_term ) [virtual]

Set the normalizing term from the combined filter as the sum of all unnormalized probabilities.

Parameters:

normalizing_term Sum of all unnormalized probabilities

Definition at line 314 of file JointFilter.cpp.

void JointFilter::setNumSamplesForLikelihoodEstimation ( int likelihood_sample_num ) [virtual]

Definition at line 304 of file JointFilter.cpp.