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

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

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

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

Public Member Functions inherited from omip::RecursiveEstimatorFilterInterface< ks_state_t, ks_measurement_t >
virtual void	addPredictedState (const StateType &predicted_state, const double &predicted_state_timestamp_ns)=0

virtual MeasurementType	getPredictedMeasurement () const

virtual StateType	getState () const

	RecursiveEstimatorFilterInterface (double loop_period_ns)

virtual void	setMeasurement (const MeasurementType &acquired_measurement, const double &measurement_timestamp)

virtual	~RecursiveEstimatorFilterInterface ()

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

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

Protected Attributes inherited from omip::RecursiveEstimatorFilterInterface< ks_state_t, ks_measurement_t >
std::string	_filter_name

double	_loop_period_ns

MeasurementType	_measurement

double	_measurement_timestamp_ns

StateType	_most_likely_predicted_state

MeasurementType	_predicted_measurement

std::vector< StateType >	_predicted_states

double	_previous_measurement_timestamp_ns

StateType	_state

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

protectedvirtual

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
	)

inlinevirtual

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 )

inlinevirtual

Definition at line 197 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setMinimumNumFramesForNewRB ( const int & value )

inlinevirtual

Definition at line 212 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setNumSamplesLikelihoodEstimation ( int likelihood_sample_num )

inlinevirtual

Definition at line 97 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticPriorCovarianceVelocity ( const double & value )

inlinevirtual

Definition at line 112 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaMeasurementNoise ( const double & value )

inlinevirtual

Definition at line 137 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoisePhi ( const double & value )

inlinevirtual

Definition at line 117 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoisePV ( const double & value )

inlinevirtual

Definition at line 127 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoisePVd ( const double & value )

inlinevirtual

Definition at line 132 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setPrismaticSigmaSystemNoiseTheta ( const double & value )

inlinevirtual

Definition at line 122 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteMaximumJointDistanceForEstimation ( const double & value )

inlinevirtual

Definition at line 192 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteMinimumRotForEstimation ( const double & value )

inlinevirtual

Definition at line 187 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevolutePriorCovarianceVelocity ( const double & value )

inlinevirtual

Definition at line 142 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaMeasurementNoise ( const double & value )

inlinevirtual

Definition at line 182 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePhi ( const double & value )

inlinevirtual

Definition at line 147 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePx ( const double & value )

inlinevirtual

Definition at line 157 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePy ( const double & value )

inlinevirtual

Definition at line 162 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoisePz ( const double & value )

inlinevirtual

Definition at line 167 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoiseRV ( const double & value )

inlinevirtual

Definition at line 172 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoiseRVd ( const double & value )

inlinevirtual

Definition at line 177 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRevoluteSigmaSystemNoiseTheta ( const double & value )

inlinevirtual

Definition at line 152 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRigidMaxRotation ( const double & value )

inlinevirtual

Definition at line 207 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setRigidMaxTranslation ( const double & value )

inlinevirtual

Definition at line 202 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setSigmaDeltaMeasurementUncertaintyAngular ( double sigma_delta_meas_uncertainty_angular )

inlinevirtual

Definition at line 107 of file MultiJointTracker.h.

virtual void omip::MultiJointTracker::setSigmaDeltaMeasurementUncertaintyLinear ( double sigma_delta_meas_uncertainty_linear )

inlinevirtual

Definition at line 102 of file MultiJointTracker.h.

Member Data Documentation

double omip::MultiJointTracker::_disconnected_j_ne

protected

Definition at line 232 of file MultiJointTracker.h.

joint_combined_filters_map omip::MultiJointTracker::_joint_combined_filters

protected

Definition at line 235 of file MultiJointTracker.h.

ks_analysis_t omip::MultiJointTracker::_ks_analysis_type

protected

Definition at line 231 of file MultiJointTracker.h.

omip_msgs::RigidBodyPosesAndVelsMsgPtr omip::MultiJointTracker::_last_rcvd_poses_and_vels

protected

Definition at line 246 of file MultiJointTracker.h.

int omip::MultiJointTracker::_likelihood_sample_num

protected

Definition at line 256 of file MultiJointTracker.h.

int omip::MultiJointTracker::_min_joint_age_for_ee

protected

Definition at line 283 of file MultiJointTracker.h.

int omip::MultiJointTracker::_min_num_frames_for_new_rb

protected

Definition at line 285 of file MultiJointTracker.h.

std::list<omip_msgs::RigidBodyPosesAndVelsMsgPtr> omip::MultiJointTracker::_n_previous_rcvd_poses_and_vels

protected

Definition at line 254 of file MultiJointTracker.h.

std::map<std::pair<int, int>, int> omip::MultiJointTracker::_precomputing_counter

protected

Definition at line 238 of file MultiJointTracker.h.

omip_msgs::RigidBodyPosesAndVelsMsgPtr omip::MultiJointTracker::_previous_rcvd_poses_and_vels

protected

Definition at line 249 of file MultiJointTracker.h.

double omip::MultiJointTracker::_prism_prior_cov_vel

protected

Definition at line 260 of file MultiJointTracker.h.

double omip::MultiJointTracker::_prism_sigma_meas_noise

protected

Definition at line 265 of file MultiJointTracker.h.

double omip::MultiJointTracker::_prism_sigma_sys_noise_phi

protected

Definition at line 261 of file MultiJointTracker.h.

double omip::MultiJointTracker::_prism_sigma_sys_noise_pv

protected

Definition at line 263 of file MultiJointTracker.h.

double omip::MultiJointTracker::_prism_sigma_sys_noise_pvd

protected

Definition at line 264 of file MultiJointTracker.h.

double omip::MultiJointTracker::_prism_sigma_sys_noise_theta

protected

Definition at line 262 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_max_joint_distance_for_ee

protected

Definition at line 278 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_min_rot_for_ee

protected

Definition at line 277 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_prior_cov_vel

protected

Definition at line 267 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_meas_noise

protected

Definition at line 275 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_sys_noise_phi

protected

Definition at line 268 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_sys_noise_px

protected

Definition at line 270 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_sys_noise_py

protected

Definition at line 271 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_sys_noise_pz

protected

Definition at line 272 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_sys_noise_rv

protected

Definition at line 273 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_sys_noise_rvd

protected

Definition at line 274 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rev_sigma_sys_noise_theta

protected

Definition at line 269 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rig_max_rotation

protected

Definition at line 281 of file MultiJointTracker.h.

double omip::MultiJointTracker::_rig_max_translation

protected

Definition at line 280 of file MultiJointTracker.h.

std::map<std::pair<int, int>, Eigen::Twistd > omip::MultiJointTracker::_rrb_pose_at_srb_birthday_in_sf

protected

Definition at line 241 of file MultiJointTracker.h.

double omip::MultiJointTracker::_sigma_delta_meas_uncertainty_angular

protected

Definition at line 258 of file MultiJointTracker.h.

double omip::MultiJointTracker::_sigma_delta_meas_uncertainty_linear

protected

Definition at line 257 of file MultiJointTracker.h.

std::map<std::pair<int, int>, Eigen::Twistd > omip::MultiJointTracker::_srb_pose_at_srb_birthday_in_sf

protected

Definition at line 244 of file MultiJointTracker.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