#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) 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	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 ()

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 RevoluteJointFilter *	doClone () const

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

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

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

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

inlineprotectedvirtual

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 871 of file RevoluteJointFilter.cpp.

std::string RevoluteJointFilter::getJointFilterTypeStr ( ) const

virtual

Return the joint type as an string

Implements omip::JointFilter.

Definition at line 876 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 623 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 552 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 524 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