RBFilter.h

Go to the documentation of this file.

/*
 * RBFilter.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 RB_FILTER_H_
#define RB_FILTER_H_

#include <rb_tracker/RBTrackerDynReconfConfig.h>

#include <pcl/point_types.h>
#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/Geometry>
#include <lgsm/Lgsm>
#include <boost/shared_ptr.hpp>
#include <boost/unordered_map.hpp>

#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistWithCovariance.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <tf_conversions/tf_eigen.h>

#include "omip_common/OMIPTypeDefs.h"
#include "omip_common/Feature.h"
#include "omip_common/FeaturesDataBase.h"

// Dimensions of the system state: 6dof of rbpose and 6dof of rbvelocity
#define STATE_DIM_RBF 6

// Dimensions of each measurement (each feature location)
#define MEAS_DIM_RBF 3

#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>


namespace omip
{

class RBFilter
{
public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // Shared Pointer to a RBFilter
    typedef boost::shared_ptr<RBFilter> Ptr;

    // Trajectory of a RB
    typedef std::vector<Eigen::Matrix4d> Trajectory;

    enum PredictionHypothesis
    {
        BASED_ON_VELOCITY = 0,
        BASED_ON_BRAKING_EVENT = 1,
        BASED_ON_KINEMATICS = 2
    };

    RBFilter();

    RBFilter(double loop_period_ns,
             const std::vector<Eigen::Matrix4d> &trajectory,
             const Eigen::Twistd& initial_velocity,
             FeaturesDataBase::Ptr feats_database,
             double estimation_error_threshold);

    virtual void Init();

    virtual ~RBFilter();

    RBFilter(const RBFilter &rbm);

    RBFilter::Ptr clone() const
    {
        return (RBFilter::Ptr(doClone()));
    }

    virtual void predictState(double time_interval_ns = -1.);

    virtual void predictMeasurement();

    virtual void correctState();

    virtual void integrateShapeBasedPose(const geometry_msgs::TwistWithCovariance twist_refinement, double pose_time_elapsed_ns);

    virtual void estimateBestPredictionAndSupportingFeatures();

    virtual void addSupportingFeature(Feature::Id supporting_feat_id);

    virtual void setPredictedState(omip_msgs::RigidBodyPoseAndVelMsg hypothesis);

    virtual void addPredictedFeatureLocation(const Feature::Location& predicted_location_velocity,
                                             const Feature::Location& predicted_location_brake, const Feature::Id feature_id);

    virtual void PredictFeatureLocation(Feature::Ptr feature,
                                        const Eigen::Matrix4d &predicted_pose,
                                        bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
                                        Feature::Location& predicted_location, bool publish_locations=false) const;

    virtual void GetLocationOfFeatureAtBirthOfRB(Feature::Ptr feature,
                                                 bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
                                                 Feature::Location& location_at_birth) const;

    virtual void setFeaturesDatabase(FeaturesDataBase::Ptr feats_database);

    virtual RB_id_t getId() const;

    virtual Eigen::Matrix4d getPose() const;

    virtual Eigen::Matrix<double, 6, 6> getPoseCovariance() const;

    virtual Eigen::Twistd getVelocity() const;

    virtual Eigen::Matrix<double, 6, 6> getVelocityCovariance() const;

    virtual geometry_msgs::PoseWithCovariancePtr getPoseWithCovariance() const;

    virtual geometry_msgs::TwistWithCovariancePtr getPoseECWithCovariance() const;

    virtual geometry_msgs::TwistWithCovariancePtr getVelocityWithCovariance() const;

    virtual std::vector<Eigen::Matrix4d> getTrajectory() const;

    virtual FeatureCloudPCLwc getPredictedMeasurement(PredictionHypothesis hypothesis = BASED_ON_VELOCITY);

    virtual std::vector<Feature::Id> getSupportingFeatures() const;

    virtual int getNumberSupportingFeatures() const;

    virtual FeaturesDataBase::Ptr getFeaturesDatabase() const;

    virtual void setMotionConstraint(int motion_constraint){}

    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 setMinNumberOfSupportingFeaturesToCorrectPredictedState(int v)
    {
        this->_min_num_supporting_feats_to_correct = v;
    }

    virtual void setEstimationThreshold(double v)
    {
        this->_estimation_error_threshold = v;
    }

    virtual Feature::Location getIntialLocationOfCentroid() const
    {
        return _initial_location_of_centroid;
    }

    virtual void doNotUsePredictionFromHigherLevel()
    {
        _use_predicted_state_from_kh = false;
    }

    FeatureCloudPCLwc::Ptr getFeaturesAtBirth()
    {
        return _features_at_birth;
    }

    FeatureCloudPCLwc::Ptr getFeaturesPredicted()
    {
        return _features_predicted;
    }

    void resetFeaturesAtBirth()
    {
        _features_at_birth->points.clear();
    }

    void resetFeaturesPredicted()
    {
        _features_predicted->points.clear();
    }

protected:

    virtual void _initializeAuxiliarMatrices();

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

    std::map<Feature::Id, double> _supporting_feats_errors;

    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;


    Eigen::MatrixXd _G_t_memory;
    Eigen::VectorXd _R_inv_times_innovation_memory;

    Eigen::Matrix<double, 3, 6> _D_T_p0_circle;

    static RB_id_t _rb_id_generator;

    RB_id_t _id;

    // Current pose of the rigid body
    Eigen::Matrix4d _pose;
    Eigen::Matrix<double, 6, 6> _pose_covariance;
    // First derivative of the pose
    Eigen::Twistd _velocity;
    Eigen::Matrix<double, 6, 6> _velocity_covariance;

    Eigen::Matrix4d _predicted_pose;
    Eigen::Matrix<double, 6, 6> _predicted_pose_covariance;
    // First derivative of the pose
    Eigen::Twistd _predicted_velocity;
    Eigen::Matrix<double, 6, 6> _predicted_velocity_covariance;

    Feature::Location _initial_location_of_centroid;
    // Sequence of Displacements of the RB
    std::vector<Eigen::Matrix4d> _trajectory;

    std::vector<Feature::Id> _supporting_features_ids;
    std::vector<double> _supporting_features_probs;

    FeaturesDataBase::Ptr _features_database;

    // Predicted poses in exponential coordinates
    Eigen::Matrix4d _predicted_pose_vh;   // Predicted pose based on the velocity hypothesis/model
    Eigen::Matrix4d _predicted_pose_bh;   // Predicted pose based on the brake-event hypothesis/model
    Eigen::Matrix4d _predicted_delta_pose_kh;   // Predicted change in pose based on the kinematic hypothesis/model
    Eigen::Matrix4d _predicted_pose_kh;   // Predicted pose based on the kinematic hypothesis/model

    Eigen::Matrix<double, 6, 6> _predicted_pose_cov_vh; // Predicted pose covariance based on the velocity hypothesis/model
    Eigen::Matrix<double, 6, 6> _predicted_pose_cov_bh; // Predicted pose covariance based on the brake hypothesis/model
    Eigen::Matrix<double, 6, 6> _predicted_pose_cov_kh; // Predicted pose covariance based on the kinematic hypothesis/model

    Eigen::Twistd _predicted_velocity_vh;   // Predicted pose based on the velocity hypothesis/model
    Eigen::Twistd _predicted_velocity_bh;   // Predicted pose based on the brake-event hypothesis/model
    Eigen::Twistd _predicted_velocity_kh;   // Predicted pose based on the kinematic hypothesis/model

    Eigen::Matrix<double, 6, 6> _predicted_velocity_cov_vh; // Predicted velocity covariance based on the velocity hypothesis/model
    Eigen::Matrix<double, 6, 6> _predicted_velocity_cov_bh; // Predicted velocity covariance based on the brake hypothesis/model
    Eigen::Matrix<double, 6, 6> _predicted_velocity_cov_kh; // Predicted velocity covariance based on the kinematic hypothesis/model

    bool _use_predicted_state_from_kh;
    bool _use_predicted_measurement_from_kh;

    FeatureCloudPCLwc::Ptr _features_predicted;
    FeatureCloudPCLwc::Ptr _features_at_birth;

    FeatureCloudPCLwc::Ptr _predicted_measurement_pc_vh;
    FeatureCloudPCLwc::Ptr _predicted_measurement_pc_bh;
    FeatureCloudPCLwc::Ptr _predicted_measurement_pc_kh;

    FeatureCloudPCLwc::Ptr _predicted_measurement;

    std::map<Feature::Id, Feature::Location> _predicted_measurement_map_vh;
    std::map<Feature::Id, Feature::Location> _predicted_measurement_map_bh;
    std::map<Feature::Id, Feature::Location> _predicted_measurement_map_kh;

    std::map<Feature::Id, Feature::Location> _predicted_measurement_map;

    double _loop_period_ns;
    double _last_time_interval_ns;

    virtual RBFilter* doClone() const
    {
        return (new RBFilter(*this));
    }

    int _num_tracked_feats;
    int _min_num_supporting_feats_to_correct;

};

}

#endif /* RB_FILTER_H_ */