Public Member Functions | Protected Attributes
omip::MultiRBTracker Class Reference

#include <MultiRBTracker.h>

Inheritance diagram for omip::MultiRBTracker:
Inheritance graph
[legend]

List of all members.

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::IdestimateBestPredictionsAndSupportingFeatures ()
std::vector< Feature::IdEstimateFreeFeatures (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::IdReassignFreeFeatures (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.

Destructor

Definition at line 123 of file MultiRBTracker.cpp.


Member Function Documentation

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.

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.

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.

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.

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.

Definition at line 671 of file MultiRBTracker.cpp.

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.

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.

Definition at line 690 of file MultiRBTracker.cpp.

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.

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.

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

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:
configValue 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_measurementLatest 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.

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.


Member Data Documentation

Definition at line 427 of file MultiRBTracker.h.

Definition at line 426 of file MultiRBTracker.h.

Definition at line 428 of file MultiRBTracker.h.

Definition at line 423 of file MultiRBTracker.h.

Definition at line 424 of file MultiRBTracker.h.

Definition at line 425 of file MultiRBTracker.h.

Definition at line 447 of file MultiRBTracker.h.

Definition at line 440 of file MultiRBTracker.h.

Definition at line 478 of file MultiRBTracker.h.

Definition at line 438 of file MultiRBTracker.h.

Definition at line 473 of file MultiRBTracker.h.

Definition at line 475 of file MultiRBTracker.h.

Definition at line 476 of file MultiRBTracker.h.

Definition at line 456 of file MultiRBTracker.h.

Definition at line 432 of file MultiRBTracker.h.

Definition at line 416 of file MultiRBTracker.h.

Definition at line 422 of file MultiRBTracker.h.

boost::mutex::scoped_lock omip::MultiRBTracker::_measurement_timestamp_ns_lock [protected]

Definition at line 483 of file MultiRBTracker.h.

Definition at line 482 of file MultiRBTracker.h.

Definition at line 435 of file MultiRBTracker.h.

Definition at line 434 of file MultiRBTracker.h.

Definition at line 419 of file MultiRBTracker.h.

Definition at line 420 of file MultiRBTracker.h.

Definition at line 421 of file MultiRBTracker.h.

Definition at line 462 of file MultiRBTracker.h.

Definition at line 465 of file MultiRBTracker.h.

Definition at line 430 of file MultiRBTracker.h.

Definition at line 469 of file MultiRBTracker.h.

Definition at line 431 of file MultiRBTracker.h.

Definition at line 453 of file MultiRBTracker.h.

Definition at line 467 of file MultiRBTracker.h.

Definition at line 417 of file MultiRBTracker.h.

Definition at line 418 of file MultiRBTracker.h.

Definition at line 443 of file MultiRBTracker.h.

Definition at line 478 of file MultiRBTracker.h.

Definition at line 480 of file MultiRBTracker.h.

Definition at line 471 of file MultiRBTracker.h.

Definition at line 450 of file MultiRBTracker.h.

Definition at line 459 of file MultiRBTracker.h.


The documentation for this class was generated from the following files:


rb_tracker
Author(s): Roberto Martín-Martín
autogenerated on Sat Jun 8 2019 18:26:42