MultiRBTracker.h

Go to the documentation of this file.

/*
 * MultiRBTracker.h
 *
 *      Author: roberto
 *
 * This is a modified implementation of the method for online estimation of kinematic structures described in our paper
 * "Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors"
 * (Martín-Martín and Brock, 2014).
 * This implementation can be used to reproduce the results of the paper and to be applied to new research.
 * The implementation allows also to be extended to perceive different information/models or to use additional sources of information.
 * A detail explanation of the method and the system can be found in our paper.
 *
 * If you are using this implementation in your research, please consider citing our work:
 *
@inproceedings{martinmartin_ip_iros_2014,
Title = {Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors},
Author = {Roberto {Mart\'in-Mart\'in} and Oliver Brock},
Booktitle = {Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems},
Pages = {2494-2501},
Year = {2014},
Location = {Chicago, Illinois, USA},
Note = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
Url = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
Projectname = {Interactive Perception}
}
 * If you have questions or suggestions, contact us:
 * roberto.martinmartin@tu-berlin.de
 *
 * Enjoy!
 */

#ifndef MULTI_RB_TRACKER_H_
#define MULTI_RB_TRACKER_H_

#include <rb_tracker/RBTrackerDynReconfConfig.h>

#include "rb_tracker/RBFilter.h"

#include "omip_common/FeaturesDataBase.h"
#include "omip_msgs/RigidBodyTwistWithCovMsg.h"

#include "omip_common/OMIPTypeDefs.h"

#include "omip_common/RecursiveEstimatorFilterInterface.h"

#include "omip_msgs/ShapeTrackerStates.h"

#include <tf/transform_listener.h>

#include "rb_tracker/StaticEnvironmentFilter.h"

#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <iostream>
#include <fstream>

#define INTEGRATOR_VISION_CHANNEL 0
#define INTEGRATOR_PROPRIOCEPTION_CHANNEL 1

namespace omip
{

class MultiRBTracker : public RecursiveEstimatorFilterInterface<rbt_state_t, rbt_measurement_t>
{
public:

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

    virtual ~MultiRBTracker();

    virtual void setMeasurement(rbt_measurement_t acquired_measurement, const double& measurement_timestamp);

    virtual void predictState(double time_interval_ns);

    virtual void predictMeasurement();


    virtual rbt_measurement_t getPredictedMeasurement() const;

    virtual void correctState();

    virtual rbt_state_t getState() const;

    void ReflectState();

    virtual void setDynamicReconfigureValues(rb_tracker::RBTrackerDynReconfConfig &config);

    virtual void processMeasurementFromShapeTracker(const omip_msgs::ShapeTrackerStates &meas_from_st);


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

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

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

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

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

    RB_id_t getRBId(int n) const;

    int getNumberSupportingFeatures(int n) const;

    FeatureCloudPCL getLabelledSupportingFeatures();

    FeatureCloudPCL getFreeFeatures();

    FeatureCloudPCL getPredictedFeatures();

    FeatureCloudPCL getAtBirthFeatures();


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

    FeaturesDataBase::Ptr getFeaturesDatabase();

    virtual void addPredictedState(const rbt_state_t& predicted_state, const double& predicted_state_timestamp_ns);

    FeatureCloudPCL::Ptr CollectLocationPredictions();

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

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

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

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

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


    virtual void setPriorCovariancePose(double v)
    {
        this->_prior_cov_pose = v;
    }

    virtual void setPriorCovarianceVelocity(double v)
    {
        this->_prior_cov_vel = v;
    }

    virtual void setMinCovarianceMeasurementX(double v)
    {
        this->_min_cov_meas_x = v;
    }

    virtual void setMinCovarianceMeasurementY(double v)
    {
        this->_min_cov_meas_y = v;
    }

    virtual void setMinCovarianceMeasurementZ(double v)
    {
        this->_min_cov_meas_z = v;
    }

    virtual void setMeasurementDepthFactor(double v)
    {
        this->_meas_depth_factor = v;
    }

    virtual void setCovarianceSystemAccelerationTx(double v)
    {
        this->_cov_sys_acc_tx = v;
    }

    virtual void setCovarianceSystemAccelerationTy(double v)
    {
        this->_cov_sys_acc_ty = v;
    }

    virtual void setCovarianceSystemAccelerationTz(double v)
    {
        this->_cov_sys_acc_tz = v;
    }

    virtual void setCovarianceSystemAccelerationRx(double v)
    {
        this->_cov_sys_acc_rx = v;
    }

    virtual void setCovarianceSystemAccelerationRy(double v)
    {
        this->_cov_sys_acc_ry = v;
    }

    virtual void setCovarianceSystemAccelerationRz(double v)
    {
        this->_cov_sys_acc_rz = v;
    }

    virtual void setNumberOfTrackedFeatures(int v)
    {
        this->_num_tracked_feats = v;
    }

    virtual void setMinNumPointsInSegment(int v)
    {
        this->_min_num_points_in_segment = v;
    }

    virtual void setMinProbabilisticValue(double v)
    {
        this->_min_probabilistic_value = v;
    }

    virtual void setMaxFitnessScore(double v)
    {
        this->_max_fitness_score = v;
    }

    virtual void setMinAmountTranslationForNewRB(double v)
    {
        this->_min_amount_translation_for_new_rb = v;
    }

    virtual void setMinAmountRotationForNewRB(double v)
    {
        this->_min_amount_rotation_for_new_rb = v;
    }

    virtual void setMinNumberOfSupportingFeaturesToCorrectPredictedState(int v)
    {
        this->_min_num_supporting_feats_to_correct = v;
    }

protected:    

    int _max_num_rb;
    double _prior_cov_pose;
    double _prior_cov_vel;
    double _min_cov_meas_x;
    double _min_cov_meas_y;
    double _min_cov_meas_z;
    double _meas_depth_factor;
    double _cov_sys_acc_tx;
    double _cov_sys_acc_ty;
    double _cov_sys_acc_tz;
    double _cov_sys_acc_ry;
    double _cov_sys_acc_rx;
    double _cov_sys_acc_rz;

    int _min_num_points_in_segment;
    double _min_probabilistic_value;
    double _max_fitness_score;

    double _min_amount_translation_for_new_rb;
    double _min_amount_rotation_for_new_rb;


    std::vector<RBFilter::Ptr> _kalman_filters;

    FeaturesDataBase::Ptr _features_db;

    // Maximum number of iterations of RANSAC to find a good hypothesis for the free features
    int _ransac_iterations;

    // Maximum error allowed between predicted and measured feature position to consider it to STILL
    // support a RBM
    double _estimation_error_threshold;

    // Minimum motion to consider that a feature moves
    double _static_motion_threshold;

    // Maximum error allowed for the inliers of a new RBM hypothesis in RANSAC
    double _new_rbm_error_threshold;

    // Maximum error allowed between the predicted and the measured position of a feature to assign it to a RBM
    double _max_error_to_reassign_feats;

    // Minimum number of features that have to support a RBM to not be deleted
    int _supporting_features_threshold;

    // Minimum number of free features to trigger the generation of a new RBM
    int _min_num_feats_for_new_rb;

    // Minimum number of frames that a features has to be present to be used to create new RBM
    int _min_num_frames_for_new_rb;

    int _num_tracked_feats;

    int _min_num_supporting_feats_to_correct;

    StaticEnvironmentFilter::Ptr _static_environment_filter;

    rbt_state_t _last_predicted_state_kh;

    double _max_distance_ee;
    double _max_distance_irb;

    std::vector<Feature::Id> _free_feat_ids, _really_free_feat_ids;

    std::ofstream _really_free_feats_file;

    boost::mutex _measurement_timestamp_ns_mutex;
    boost::mutex::scoped_lock _measurement_timestamp_ns_lock;

};
}

#endif /* MULTI_RB_TRACKER_H_ */