#include <MultiJointTracker.h>

Inheritance diagram for omip::MultiJointTracker:

Public Member Functions
virtual void	addPredictedState (const ks_state_t &predicted_state, const double &predicted_state_timestamp_ns)
virtual void	correctState ()
	Third and final step when updating the filter. The predicted next state is corrected based on the difference between predicted and acquired measurement.
virtual void	estimateJointFiltersProbabilities ()
virtual JointCombinedFilterPtr	getCombinedFilter (int n)
	MultiJointTracker (double loop_period_ns, ks_analysis_t ks_analysis_type, double dj_ne)
virtual void	predictMeasurement ()
	Second step when updating the filter. The next measurement is predicted from the predicted next state.
virtual void	predictState (double time_interval_ns)
	First step when updating the filter. The next state is predicted from current state and system model.
virtual void	setMeasurement (const ks_measurement_t &poses_and_vels, const double &measurement_timestamp_ns)
virtual void	setMinimumJointAgeForEE (const int &value)
virtual void	setMinimumNumFramesForNewRB (const int &value)
virtual void	setNumSamplesLikelihoodEstimation (int likelihood_sample_num)
virtual void	setPrismaticPriorCovarianceVelocity (const double &value)
virtual void	setPrismaticSigmaMeasurementNoise (const double &value)
virtual void	setPrismaticSigmaSystemNoisePhi (const double &value)
virtual void	setPrismaticSigmaSystemNoisePV (const double &value)
virtual void	setPrismaticSigmaSystemNoisePVd (const double &value)
virtual void	setPrismaticSigmaSystemNoiseTheta (const double &value)
virtual void	setRevoluteMaximumJointDistanceForEstimation (const double &value)
virtual void	setRevoluteMinimumRotForEstimation (const double &value)
virtual void	setRevolutePriorCovarianceVelocity (const double &value)
virtual void	setRevoluteSigmaMeasurementNoise (const double &value)
virtual void	setRevoluteSigmaSystemNoisePhi (const double &value)
virtual void	setRevoluteSigmaSystemNoisePx (const double &value)
virtual void	setRevoluteSigmaSystemNoisePy (const double &value)
virtual void	setRevoluteSigmaSystemNoisePz (const double &value)
virtual void	setRevoluteSigmaSystemNoiseRV (const double &value)
virtual void	setRevoluteSigmaSystemNoiseRVd (const double &value)
virtual void	setRevoluteSigmaSystemNoiseTheta (const double &value)
virtual void	setRigidMaxRotation (const double &value)
virtual void	setRigidMaxTranslation (const double &value)
virtual void	setSigmaDeltaMeasurementUncertaintyAngular (double sigma_delta_meas_uncertainty_angular)
virtual void	setSigmaDeltaMeasurementUncertaintyLinear (double sigma_delta_meas_uncertainty_linear)
virtual	~MultiJointTracker ()
Protected Member Functions
virtual void	_reflectState ()
	Prepares the state variable to be returned (getState is constant and we cannot change the state there) The estimated state is the kinemetic structure: for each pair of RBids the most likely JointFilter object, which includes the joint parameters and the latent variables.
Protected Attributes
double	_disconnected_j_ne
joint_combined_filters_map	_joint_combined_filters
ks_analysis_t	_ks_analysis_type
omip_msgs::RigidBodyPosesAndVelsMsgPtr	_last_rcvd_poses_and_vels
int	_likelihood_sample_num
int	_min_joint_age_for_ee
int	_min_num_frames_for_new_rb
std::list < omip_msgs::RigidBodyPosesAndVelsMsgPtr >	_n_previous_rcvd_poses_and_vels
std::map< std::pair< int, int > , int >	_precomputing_counter
omip_msgs::RigidBodyPosesAndVelsMsgPtr	_previous_rcvd_poses_and_vels
double	_prism_prior_cov_vel
double	_prism_sigma_meas_noise
double	_prism_sigma_sys_noise_phi
double	_prism_sigma_sys_noise_pv
double	_prism_sigma_sys_noise_pvd
double	_prism_sigma_sys_noise_theta
double	_rev_max_joint_distance_for_ee
double	_rev_min_rot_for_ee
double	_rev_prior_cov_vel
double	_rev_sigma_meas_noise
double	_rev_sigma_sys_noise_phi
double	_rev_sigma_sys_noise_px
double	_rev_sigma_sys_noise_py
double	_rev_sigma_sys_noise_pz
double	_rev_sigma_sys_noise_rv
double	_rev_sigma_sys_noise_rvd
double	_rev_sigma_sys_noise_theta
double	_rig_max_rotation
double	_rig_max_translation
std::map< std::pair< int, int > , Eigen::Twistd >	_rrb_pose_at_srb_birthday_in_sf
double	_sigma_delta_meas_uncertainty_angular
double	_sigma_delta_meas_uncertainty_linear
std::map< std::pair< int, int > , Eigen::Twistd >	_srb_pose_at_srb_birthday_in_sf

Detailed Description

Definition at line 61 of file MultiJointTracker.h.

Constructor & Destructor Documentation

MultiJointTracker::MultiJointTracker	(	double	loop_period_ns,
		ks_analysis_t	ks_analysis_type,
		double	dj_ne
	)

Definition at line 18 of file MultiJointTracker.cpp.

MultiJointTracker::~MultiJointTracker ( ) [virtual]

Definition at line 29 of file MultiJointTracker.cpp.

Member Function Documentation

void MultiJointTracker::_reflectState ( ) [protected, virtual]

Prepares the state variable to be returned (getState is constant and we cannot change the state there) The estimated state is the kinemetic structure: for each pair of RBids the most likely JointFilter object, which includes the joint parameters and the latent variables.

Definition at line 318 of file MultiJointTracker.cpp.

virtual void omip::MultiJointTracker::addPredictedState	(	const ks_state_t &	predicted_state,
		const double &	predicted_state_timestamp_ns
	)		`[inline, virtual]`

Definition at line 92 of file MultiJointTracker.h.

void MultiJointTracker::correctState ( ) [virtual]

Third and final step when updating the filter. The predicted next state is corrected based on the difference between predicted and acquired measurement.

Implements omip::RecursiveEstimatorFilterInterface< ks_state_t, ks_measurement_t >.

Definition at line 170 of file MultiJointTracker.cpp.

void MultiJointTracker::estimateJointFiltersProbabilities ( ) [virtual]

Definition at line 324 of file MultiJointTracker.cpp.

JointCombinedFilterPtr MultiJointTracker::getCombinedFilter ( int n ) [virtual]

Definition at line 159 of file MultiJointTracker.cpp.

void MultiJointTracker::predictMeasurement ( ) [virtual]

Second step when updating the filter. The next measurement is predicted from the predicted next state.

Implements omip::RecursiveEstimatorFilterInterface< ks_state_t, ks_measurement_t >.

Definition at line 42 of file MultiJointTracker.cpp.

void MultiJointTracker::predictState ( double time_interval_ns ) [virtual]

First step when updating the filter. The next state is predicted from current state and system model.

Implements omip::RecursiveEstimatorFilterInterface< ks_state_t, ks_measurement_t >.

Definition at line 34 of file MultiJointTracker.cpp.

void MultiJointTracker::setMeasurement	(	const ks_measurement_t &	poses_and_vels,
		const double &	measurement_timestamp_ns
	)		`[virtual]`

Set the last received poses of the RBs

Definition at line 102 of file MultiJointTracker.cpp.

virtual void omip::MultiJointTracker::setMinimumJointAgeForEE ( const int & value ) [inline, virtual]

Definition at line 197 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setMinimumNumFramesForNewRB ( const int & value ) [inline, virtual]

Definition at line 212 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setNumSamplesLikelihoodEstimation ( int likelihood_sample_num ) [inline, virtual]

Definition at line 97 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticPriorCovarianceVelocity ( const double & value ) [inline, virtual]

Definition at line 112 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaMeasurementNoise ( const double & value ) [inline, virtual]

Definition at line 137 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoisePhi ( const double & value ) [inline, virtual]

Definition at line 117 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoisePV ( const double & value ) [inline, virtual]

Definition at line 127 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoisePVd ( const double & value ) [inline, virtual]

Definition at line 132 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoiseTheta ( const double & value ) [inline, virtual]

Definition at line 122 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteMaximumJointDistanceForEstimation ( const double & value ) [inline, virtual]

Definition at line 192 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteMinimumRotForEstimation ( const double & value ) [inline, virtual]

Definition at line 187 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevolutePriorCovarianceVelocity ( const double & value ) [inline, virtual]

Definition at line 142 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaMeasurementNoise ( const double & value ) [inline, virtual]

Definition at line 182 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePhi ( const double & value ) [inline, virtual]

Definition at line 147 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePx ( const double & value ) [inline, virtual]

Definition at line 157 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePy ( const double & value ) [inline, virtual]

Definition at line 162 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePz ( const double & value ) [inline, virtual]

Definition at line 167 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoiseRV ( const double & value ) [inline, virtual]

Definition at line 172 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoiseRVd ( const double & value ) [inline, virtual]

Definition at line 177 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoiseTheta ( const double & value ) [inline, virtual]

Definition at line 152 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRigidMaxRotation ( const double & value ) [inline, virtual]

Definition at line 207 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRigidMaxTranslation ( const double & value ) [inline, virtual]

Definition at line 202 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setSigmaDeltaMeasurementUncertaintyAngular ( double sigma_delta_meas_uncertainty_angular ) [inline, virtual]

Definition at line 107 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setSigmaDeltaMeasurementUncertaintyLinear ( double sigma_delta_meas_uncertainty_linear ) [inline, virtual]

Definition at line 102 of file MultiJointTracker.h.