#include <RigidJointFilter.h>

Inheritance diagram for omip::RigidJointFilter:

Public Member Functions
RigidJointFilterPtr	clone () const

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

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

	RigidJointFilter ()

	RigidJointFilter (const RigidJointFilter &rigid_joint)

virtual void	setMaxRotationRigid (double max_rot)

virtual void	setMaxTranslationRigid (double max_trans)

virtual	~RigidJointFilter ()

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 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. 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	predictState (double time_interval_ns)
	Predict the next state. 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
virtual RigidJointFilter *	doClone () const

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

Protected Attributes
double	_motion_memory_prior

double	_rig_max_rotation

double	_rig_max_translation

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 43 of file RigidJointFilter.h.

Constructor & Destructor Documentation

RigidJointFilter::RigidJointFilter ( )

Constructor

Definition at line 5 of file RigidJointFilter.cpp.

RigidJointFilter::~RigidJointFilter ( )

virtual

Destructor

Definition at line 13 of file RigidJointFilter.cpp.

RigidJointFilter::RigidJointFilter ( const RigidJointFilter & rigid_joint )

Copy constructor

Definition at line 18 of file RigidJointFilter.cpp.

Member Function Documentation

RigidJointFilterPtr omip::RigidJointFilter::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 63 of file RigidJointFilter.h.

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

inlineprotectedvirtual

A Rigid Joint constrains the 6 dofs of the relative motion between RBs Rigid Joints have no motion parameters Rigid Joints have no latent variables

Implements omip::JointFilter.

Definition at line 139 of file RigidJointFilter.h.

void RigidJointFilter::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 51 of file RigidJointFilter.cpp.

void RigidJointFilter::estimateUnnormalizedModelProbability ( )

virtual

Reimplemented from omip::JointFilter.

Definition at line 127 of file RigidJointFilter.cpp.

JointFilterType RigidJointFilter::getJointFilterType ( ) const

virtual

Return the joint type as one of the possible JointFilterTypes

Implements omip::JointFilter.

Definition at line 315 of file RigidJointFilter.cpp.

std::string RigidJointFilter::getJointFilterTypeStr ( ) const

virtual

Return the joint type as an string

Implements omip::JointFilter.

Definition at line 320 of file RigidJointFilter.cpp.

std::vector< visualization_msgs::Marker > RigidJointFilter::getJointMarkersInRRBFrame ( ) const

virtual

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

Implements omip::JointFilter.

Definition at line 239 of file RigidJointFilter.cpp.

geometry_msgs::TwistWithCovariance RigidJointFilter::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 132 of file RigidJointFilter.cpp.

geometry_msgs::TwistWithCovariance RigidJointFilter::getPredictedSRBPoseWithCovInSensorFrame ( )

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)

Implements omip::JointFilter.

Definition at line 184 of file RigidJointFilter.cpp.

geometry_msgs::TwistWithCovariance RigidJointFilter::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 158 of file RigidJointFilter.cpp.

void RigidJointFilter::initialize ( )

virtual

Reimplemented from omip::JointFilter.

Definition at line 23 of file RigidJointFilter.cpp.

void RigidJointFilter::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 40 of file RigidJointFilter.cpp.

void RigidJointFilter::setMaxRotationRigid ( double max_rot )

virtual

Definition at line 35 of file RigidJointFilter.cpp.

void RigidJointFilter::setMaxTranslationRigid ( double max_trans )

virtual

Definition at line 30 of file RigidJointFilter.cpp.

Member Data Documentation

double omip::RigidJointFilter::_motion_memory_prior

protected

Definition at line 147 of file RigidJointFilter.h.

double omip::RigidJointFilter::_rig_max_rotation

protected

Definition at line 145 of file RigidJointFilter.h.

double omip::RigidJointFilter::_rig_max_translation

protected

Definition at line 144 of file RigidJointFilter.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