#include <MultiRBTracker.h>

Inheritance diagram for omip::MultiRBTracker:

Public Member Functions
void	addFeatureLocation (Feature::Id f_id, Feature::Location f_loc)
virtual void	addPredictedState (const rbt_state_t &predicted_state, const double &predicted_state_timestamp_ns)
FeatureCloudPCL::Ptr	CollectLocationPredictions ()
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 1- Estimate the supporting Features of each filter: 1.1- Check if the previously supporting Features still support each filter 1.2- Delete filters that do not have enough support 1.3- Assign moving Features that do not support any filter yet 2- Correct the predicted next state of each filter using the assigned Features and the Measurement Model.
void	CreateNewFilter (const std::vector< Eigen::Matrix4d > &initial_trajectory, const Eigen::Twistd &initial_velocity, const std::vector< Feature::Id > &initial_supporting_feats)
std::vector< Feature::Id >	estimateBestPredictionsAndSupportingFeatures ()
std::vector< Feature::Id >	EstimateFreeFeatures (const std::vector< Feature::Id > &supporting_features)
FeatureCloudPCL	getAtBirthFeatures ()
std::vector< Eigen::Vector3d >	getCentroids () const
FeaturesDataBase::Ptr	getFeaturesDatabase ()
FeatureCloudPCL	getFreeFeatures ()
FeatureCloudPCL	getLabelledSupportingFeatures ()
int	getNumberSupportingFeatures (int n) const
std::vector< Eigen::Matrix4d >	getPoses () const
std::vector < geometry_msgs::TwistWithCovariancePtr >	getPosesECWithCovariance () const
std::vector < geometry_msgs::PoseWithCovariancePtr >	getPosesWithCovariance () const
FeatureCloudPCL	getPredictedFeatures ()
virtual rbt_measurement_t	getPredictedMeasurement () const
	Get the predicted next measurement.
RB_id_t	getRBId (int n) const
virtual rbt_state_t	getState () const
	Get the currently belief state.
std::vector < geometry_msgs::TwistWithCovariancePtr >	getVelocitiesWithCovariance () const
virtual void	Init ()
	MultiRBTracker (int max_num_rb, double loop_period_ns, int ransac_iterations, double estimation_error_threshold, double static_motion_threshold, double new_rbm_error_threshold, double max_error_to_reassign_feats, int supporting_features_threshold, int min_num_feats_for_new_rb, int min_num_frames_for_new_rb, int initial_cam_motion_constraint, static_environment_tracker_t static_environment_tracker_type)
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 (part of the RecursiveEstimatorInterace) In the FeatureTracker the system model is the most simple one: no change in the state. So the predicted state is just the current state.
virtual void	processMeasurementFromShapeTracker (const omip_msgs::ShapeTrackerStates &meas_from_st)
	Process the measurement coming from the ShapeTrackerNode that is a refinement/substitute of the feature based tracking.
std::vector< Feature::Id >	ReassignFreeFeatures (const std::vector< Feature::Id > &free_feat_ids)
void	ReflectState ()
	Prepare the corrected state to be published.
virtual void	setCovarianceSystemAccelerationRx (double v)
virtual void	setCovarianceSystemAccelerationRy (double v)
virtual void	setCovarianceSystemAccelerationRz (double v)
virtual void	setCovarianceSystemAccelerationTx (double v)
virtual void	setCovarianceSystemAccelerationTy (double v)
virtual void	setCovarianceSystemAccelerationTz (double v)
virtual void	setDynamicReconfigureValues (rb_tracker::RBTrackerDynReconfConfig &config)
	Dynamic configuration.
virtual void	setMaxFitnessScore (double v)
virtual void	setMeasurement (rbt_measurement_t acquired_measurement, const double &measurement_timestamp)
	Set the latest acquired measurement.
virtual void	setMeasurementDepthFactor (double v)
virtual void	setMinAmountRotationForNewRB (double v)
virtual void	setMinAmountTranslationForNewRB (double v)
virtual void	setMinCovarianceMeasurementX (double v)
virtual void	setMinCovarianceMeasurementY (double v)
virtual void	setMinCovarianceMeasurementZ (double v)
virtual void	setMinNumberOfSupportingFeaturesToCorrectPredictedState (int v)
virtual void	setMinNumPointsInSegment (int v)
virtual void	setMinProbabilisticValue (double v)
virtual void	setNumberOfTrackedFeatures (int v)
virtual void	setPriorCovariancePose (double v)
virtual void	setPriorCovarianceVelocity (double v)
void	TryToCreateNewFilter (const std::vector< Feature::Id > &really_free_feat_ids)
virtual	~MultiRBTracker ()
Protected Attributes
double	_cov_sys_acc_rx
double	_cov_sys_acc_ry
double	_cov_sys_acc_rz
double	_cov_sys_acc_tx
double	_cov_sys_acc_ty
double	_cov_sys_acc_tz
double	_estimation_error_threshold
FeaturesDataBase::Ptr	_features_db
std::vector< Feature::Id >	_free_feat_ids
std::vector< RBFilter::Ptr >	_kalman_filters
rbt_state_t	_last_predicted_state_kh
double	_max_distance_ee
double	_max_distance_irb
double	_max_error_to_reassign_feats
double	_max_fitness_score
int	_max_num_rb
double	_meas_depth_factor
boost::mutex::scoped_lock	_measurement_timestamp_ns_lock
boost::mutex	_measurement_timestamp_ns_mutex
double	_min_amount_rotation_for_new_rb
double	_min_amount_translation_for_new_rb
double	_min_cov_meas_x
double	_min_cov_meas_y
double	_min_cov_meas_z
int	_min_num_feats_for_new_rb
int	_min_num_frames_for_new_rb
int	_min_num_points_in_segment
int	_min_num_supporting_feats_to_correct
double	_min_probabilistic_value
double	_new_rbm_error_threshold
int	_num_tracked_feats
double	_prior_cov_pose
double	_prior_cov_vel
int	_ransac_iterations
std::vector< Feature::Id >	_really_free_feat_ids
std::ofstream	_really_free_feats_file
StaticEnvironmentFilter::Ptr	_static_environment_filter
double	_static_motion_threshold
int	_supporting_features_threshold

Detailed Description

Class MultiRBTracker This class manages a set of RBFilters that tracks the rigid bodies in the environment The MultiRBTracker: 1- receives the 3D tracked Features and assigns them to support RBFilters 2- steps existing RBFilters using the supporting Features 3- creates new RBFilters if there are enough moving Features that do not support any RBFilter

Definition at line 72 of file MultiRBTracker.h.

Constructor & Destructor Documentation

MultiRBTracker::MultiRBTracker	(	int	max_num_rb,
		double	loop_period_ns,
		int	ransac_iterations,
		double	estimation_error_threshold,
		double	static_motion_threshold,
		double	new_rbm_error_threshold,
		double	max_error_to_reassign_feats,
		int	supporting_features_threshold,
		int	min_num_feats_for_new_rb,
		int	min_num_frames_for_new_rb,
		int	initial_cam_motion_constraint,
		static_environment_tracker_t	static_environment_tracker_type
	)

Constructor

Parameters:

ransac_iterations	- Number of iterations of the RanSaC algorithm that creates new RBFilters from not assigned moving Features
estimation_error_threshold	- Maximum error between predicted and measured Feature Location to consider that this Feature still supports the RBFilter that generated the prediction
static_motion_threshold	- Minimum motion from its initial Location to consider a Feature to be moving
new_rbm_error_threshold	- Maximum error in the RanSaC algorithm for the creation of new RBFilters to consider that a moving Feature is an inlier
max_error_to_reassign_feats	- Maximum error between predicted and measured Feature Location to assign a Feature to support the RBFilter that generated the prediction
supporting_features_threshold	- Minimum number of supporting Features that a RBFilter has to have to remain alive
min_num_feats_for_new_rb	- Minimum number of moving Features to trigger the creation of a new RBFilter
min_num_frames_for_new_rb	- Minimum number of frames that a Feature has to be tracked to be used for the creation of new RBFilters

Definition at line 52 of file MultiRBTracker.cpp.

MultiRBTracker::~MultiRBTracker ( ) [virtual]

Destructor

Definition at line 123 of file MultiRBTracker.cpp.

Member Function Documentation

void MultiRBTracker::addFeatureLocation	(	Feature::Id	f_id,
		Feature::Location	f_loc
	)

Add the last tracked Location of a Feature to the data base

Parameters:

f_id	- Id of the tracked Feature
f_loc	- New Location of the tracked Feature

Definition at line 623 of file MultiRBTracker.cpp.

void MultiRBTracker::addPredictedState	(	const rbt_state_t &	predicted_state,
		const double &	predicted_state_timestamp_ns
	)		`[virtual]`

Pass a set of new predictions of the next pose and velocity of each rigid body, computed by the higher level (JointTracker) Implements the feedback from higher inference levels (JointTracker)

Parameters:

twist_wc - Predictions about the next poses of the rigid bodies and their covariances

Definition at line 771 of file MultiRBTracker.cpp.

FeatureCloudPCL::Ptr omip::MultiRBTracker::CollectLocationPredictions ( )

Collect the predictions about the next Feature Locations from each RBFilter Implements the feedback to lower inference levels (FeatureTracker)

Returns:: - Shared pointer to a point cloud with the predicted next Feature Locations

void MultiRBTracker::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 1- Estimate the supporting Features of each filter: 1.1- Check if the previously supporting Features still support each filter 1.2- Delete filters that do not have enough support 1.3- Assign moving Features that do not support any filter yet 2- Correct the predicted next state of each filter using the assigned Features and the Measurement Model.

Implements omip::RecursiveEstimatorFilterInterface< rbt_state_t, rbt_measurement_t >.

Definition at line 259 of file MultiRBTracker.cpp.

void MultiRBTracker::CreateNewFilter	(	const std::vector< Eigen::Matrix4d > &	initial_trajectory,
		const Eigen::Twistd &	initial_velocity,
		const std::vector< Feature::Id > &	initial_supporting_feats
	)

Create a new RBFilter

Parameters:

initial_trajectory	- Initial set of poses of the rigid body (exponential coordinates)
initial_velocity_guess	- Initial velocity of the rigid body (twist)
initial_supporting_feats	- Vector with the Ids of the Features that support the new RBFilter

Definition at line 350 of file MultiRBTracker.cpp.

std::vector< Feature::Id > MultiRBTracker::estimateBestPredictionsAndSupportingFeatures ( )

Estimate the set of supporting Features of each Filter. Right now it just checks that the supporting Features of each Filter STILL support it.

Returns:: - Vector of Ids of the supporting Features

Definition at line 220 of file MultiRBTracker.cpp.

std::vector< Feature::Id > MultiRBTracker::EstimateFreeFeatures ( const std::vector< Feature::Id > & supporting_features )

Estimate the set of free Features as the Features that are moving (enough motion from its initial Location) but do not support any existing RBFilter

Parameters:

supporting_features - Vector of Ids of the features that support one RBFilter (not free)

Returns:: - Vector of Ids of the free Features (= alive_features - supporting_features)

Definition at line 384 of file MultiRBTracker.cpp.

FeatureCloudPCL MultiRBTracker::getAtBirthFeatures ( )

Definition at line 712 of file MultiRBTracker.cpp.

std::vector< Eigen::Vector3d > MultiRBTracker::getCentroids ( ) const

Get the centroids of each RBFilter as the centroid of the last Location of the Features that support them. This centroids are used only for visualization purposes: they are used to place the prismatic joints markers

Returns:: - Vector with the 3D centroid of each RBFilter

Definition at line 734 of file MultiRBTracker.cpp.

FeaturesDataBase::Ptr MultiRBTracker::getFeaturesDatabase ( )

Get the shared pointer to the database of Features that can be used for both the FeatureTracker and the next level in IP, the MultiRBTracker. The FeatureTracker can directly update the Database of Features and it will be aware of the Feature Ids which simplifies the use of predicted next Feature Locations

Returns:: - Shared pointer to DataBase of Features

Definition at line 786 of file MultiRBTracker.cpp.

FeatureCloudPCL MultiRBTracker::getFreeFeatures ( )

Definition at line 671 of file MultiRBTracker.cpp.

FeatureCloudPCL MultiRBTracker::getLabelledSupportingFeatures ( )

Get a point cloud with the last Location of the supporting Features labeled by the RBFilter they support (kind of segmentation)

Returns:: - Point cloud with the labeled supporting Features

Definition at line 642 of file MultiRBTracker.cpp.

int MultiRBTracker::getNumberSupportingFeatures ( int n ) const

Get the number of supporting features of the RBFilter at a certain position in the list of alive Filters

Parameters:

n	- Position in the list of alive Filters from where we want to obtain the Id

Returns:: - Number of supporting features of the Filter

Definition at line 637 of file MultiRBTracker.cpp.

std::vector< Eigen::Matrix4d > MultiRBTracker::getPoses ( ) const

Get tracked poses from each RBFilter

Returns:: - Vector of poses of each tracked rigid body

Definition at line 582 of file MultiRBTracker.cpp.

std::vector< geometry_msgs::TwistWithCovariancePtr > MultiRBTracker::getPosesECWithCovariance ( ) const

Get tracked poses from each RBFilter (exponential coordinates) and their covariances

Returns:: - Vector of shared pointers to poses (exponential coordinates) and covariances of each tracked rigid body

Definition at line 603 of file MultiRBTracker.cpp.

std::vector< geometry_msgs::PoseWithCovariancePtr > MultiRBTracker::getPosesWithCovariance ( ) const

Get tracked poses from each RBFilter (translation vector + quaternion) and their covariances

Returns:: - Vector of shared pointers to poses (translation vector + quaternion) and covariances of each tracked rigid body

Definition at line 593 of file MultiRBTracker.cpp.

FeatureCloudPCL MultiRBTracker::getPredictedFeatures ( )

Definition at line 690 of file MultiRBTracker.cpp.

rbt_measurement_t MultiRBTracker::getPredictedMeasurement ( ) const [virtual]

Get the predicted next measurement.

Returns:: rbt_measurement_t rbt_measurement_t Predicted next measurement based on the predicted next state

Reimplemented from omip::RecursiveEstimatorFilterInterface< rbt_state_t, rbt_measurement_t >.

Definition at line 214 of file MultiRBTracker.cpp.

RB_id_t MultiRBTracker::getRBId ( int n ) const

Get the unique Id of the RBFilter at a certain position in the list of alive Filters

Parameters:

n	- Position in the list of alive Filters from where we want to obtain the Id

Returns:: - Id of the RBFilter at that position in the list

Definition at line 632 of file MultiRBTracker.cpp.

rbt_state_t MultiRBTracker::getState ( ) const [virtual]

Get the currently belief state.

Returns:: StateType Currently belief state

Reimplemented from omip::RecursiveEstimatorFilterInterface< rbt_state_t, rbt_measurement_t >.

Definition at line 314 of file MultiRBTracker.cpp.

std::vector< geometry_msgs::TwistWithCovariancePtr > MultiRBTracker::getVelocitiesWithCovariance ( ) const

Get tracked velocities from each RBFilter (twists) and their covariances

Returns:: - Vector of shared pointers to velocities (twists) and covariances of each tracked rigid body

Definition at line 613 of file MultiRBTracker.cpp.

void MultiRBTracker::Init ( ) [virtual]

Definition at line 104 of file MultiRBTracker.cpp.

void MultiRBTracker::predictMeasurement ( ) [virtual]

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

Implements omip::RecursiveEstimatorFilterInterface< rbt_state_t, rbt_measurement_t >.

Definition at line 176 of file MultiRBTracker.cpp.

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

First step when updating the filter. The next state is predicted from current state and system model (part of the RecursiveEstimatorInterace) In the FeatureTracker the system model is the most simple one: no change in the state. So the predicted state is just the current state.

Implements omip::RecursiveEstimatorFilterInterface< rbt_state_t, rbt_measurement_t >.

Definition at line 157 of file MultiRBTracker.cpp.

void MultiRBTracker::processMeasurementFromShapeTracker ( const omip_msgs::ShapeTrackerStates & meas_from_st ) [virtual]

Process the measurement coming from the ShapeTrackerNode that is a refinement/substitute of the feature based tracking.

Parameters:

meas_from_st Measurement arriving from the shape-based tracker

Definition at line 326 of file MultiRBTracker.cpp.

std::vector< Feature::Id > MultiRBTracker::ReassignFreeFeatures ( const std::vector< Feature::Id > & free_feat_ids )

Try to assign free Features (Features that are moving but do not support any existing RBFilter) to one of the existing RBFilters

Parameters:

free_feat_ids - Vector of Ids of the free Features (= alive_features - supporting_features)

Returns:: - Vector of Ids of the remaining free Features: Features that could not be reassigned to any existing Filter

Definition at line 404 of file MultiRBTracker.cpp.

void MultiRBTracker::ReflectState ( )

Prepare the corrected state to be published.

Definition at line 286 of file MultiRBTracker.cpp.

virtual void omip::MultiRBTracker::setCovarianceSystemAccelerationRx ( double v ) [inline, virtual]

Definition at line 364 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setCovarianceSystemAccelerationRy ( double v ) [inline, virtual]

Definition at line 369 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setCovarianceSystemAccelerationRz ( double v ) [inline, virtual]

Definition at line 374 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setCovarianceSystemAccelerationTx ( double v ) [inline, virtual]

Definition at line 349 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setCovarianceSystemAccelerationTy ( double v ) [inline, virtual]

Definition at line 354 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setCovarianceSystemAccelerationTz ( double v ) [inline, virtual]

Definition at line 359 of file MultiRBTracker.h.

void MultiRBTracker::setDynamicReconfigureValues ( rb_tracker::RBTrackerDynReconfConfig & config ) [virtual]

Dynamic configuration.

Parameters:

config Value from Dynamic Reconfigure

Definition at line 319 of file MultiRBTracker.cpp.

virtual void omip::MultiRBTracker::setMaxFitnessScore ( double v ) [inline, virtual]

Definition at line 394 of file MultiRBTracker.h.

void MultiRBTracker::setMeasurement	(	rbt_measurement_t	acquired_measurement,
		const double &	measurement_timestamp
	)		`[virtual]`

Set the latest acquired measurement.

Parameters:

acquired_measurement Latest acquired measurement

Definition at line 128 of file MultiRBTracker.cpp.

virtual void omip::MultiRBTracker::setMeasurementDepthFactor ( double v ) [inline, virtual]

Definition at line 344 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinAmountRotationForNewRB ( double v ) [inline, virtual]

Definition at line 404 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinAmountTranslationForNewRB ( double v ) [inline, virtual]

Definition at line 399 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinCovarianceMeasurementX ( double v ) [inline, virtual]

Definition at line 329 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinCovarianceMeasurementY ( double v ) [inline, virtual]

Definition at line 334 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinCovarianceMeasurementZ ( double v ) [inline, virtual]

Definition at line 339 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinNumberOfSupportingFeaturesToCorrectPredictedState ( int v ) [inline, virtual]

Definition at line 409 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinNumPointsInSegment ( int v ) [inline, virtual]

Definition at line 384 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setMinProbabilisticValue ( double v ) [inline, virtual]

Definition at line 389 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setNumberOfTrackedFeatures ( int v ) [inline, virtual]

Definition at line 379 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setPriorCovariancePose ( double v ) [inline, virtual]

Definition at line 319 of file MultiRBTracker.h.

virtual void omip::MultiRBTracker::setPriorCovarianceVelocity ( double v ) [inline, virtual]

Definition at line 324 of file MultiRBTracker.h.

void MultiRBTracker::TryToCreateNewFilter ( const std::vector< Feature::Id > & really_free_feat_ids )

Try to create a new RBFilter from a set of free Features that could not be reassigned to any existing RBFilter The process is: 1- Take the free Features that are old enough (have been tracked at least during N frames) 2- Take the last location and location N frames before of the old enough Features 3- Run RanSaC on between these two location sets: 3.1- Pick up randomly 4 pairs of last-NToLast Locations 3.2- Estimate the homogeneous transform (HT) that explains the motion of these 4 Features 3.3- Compute the inliers of this HT, other free Features that also moved according to this HT. To count as inlier the error between predicted location (resulting of applying the HT to the NToLast location) and measured Location (last Location) must be under a threshold (new_rbm_error_threshold) 3.4- If enough Features are inliers of an HT we compute the HT that explains the motion of all inliers between last and NextToLast frames and create a new RBFilter using this HT as initial pose and initial velocity. We compute the HT between the last 2 frames so that the HTs are always at the same time steps.

Parameters:

really_free_feat_ids

Definition at line 461 of file MultiRBTracker.cpp.