RevoluteJointFilter.h

Go to the documentation of this file.

/*
 * RevoluteJointFilter.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 REVOLUTEJOINTFILTER_H_
#define REVOLUTEJOINTFILTER_H_

#include "joint_tracker/JointFilter.h"

// Bayesian Filtering ROS
#include <pdf/linearanalyticconditionalgaussian.h>
#include <pdf/analyticconditionalgaussian.h>
#include <pdf/analyticconditionalgaussian_additivenoise.h>
#include <wrappers/matrix/matrix_wrapper.h>
#include <filter/extendedkalmanfilter.h>
#include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <model/analyticmeasurementmodel_gaussianuncertainty.h>
#include <model/measurementmodel.h>
#include "joint_tracker/pdf/NonLinearRevoluteMeasurementPdf.h"

namespace omip
{

class RevoluteJointFilter;
typedef boost::shared_ptr<RevoluteJointFilter> RevoluteJointFilterPtr;

class RevoluteJointFilter : public JointFilter
{
public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    RevoluteJointFilter();

    virtual ~RevoluteJointFilter();

    RevoluteJointFilterPtr clone() const
    {
        return (RevoluteJointFilterPtr(doClone()));
    }

    RevoluteJointFilter(const RevoluteJointFilter &rev_joint);

    virtual void predictState(double time_interval_ns);

    virtual void predictMeasurement();

    virtual void correctState();

    virtual void estimateMeasurementHistoryLikelihood();

    virtual geometry_msgs::TwistWithCovariance getPredictedSRBPoseWithCovInSensorFrame();

    virtual void estimateUnnormalizedModelProbability() ;

    virtual geometry_msgs::TwistWithCovariance getPredictedSRBDeltaPoseWithCovInSensorFrame();

    virtual geometry_msgs::TwistWithCovariance getPredictedSRBVelocityWithCovInSensorFrame();


    virtual std::vector<visualization_msgs::Marker> getJointMarkersInRRBFrame() const;

    virtual JointFilterType getJointFilterType() const;

    virtual std::string getJointFilterTypeStr() const;

    virtual void setMinRotationRevolute(const double& value);

    virtual void setMaxRadiusDistanceRevolute(const double& value);

    virtual void initialize();

    virtual void setCovarianceDeltaMeasurementAngular(double sigma_delta_meas_uncertainty_angular);

protected:

    double _sigma_delta_meas_uncertainty_angular;

    double _rev_min_rot_for_ee;
    double _rev_max_joint_distance_for_ee;

    BFL::LinearAnalyticConditionalGaussian* _sys_PDF;
    BFL::LinearAnalyticSystemModelGaussianUncertainty* _sys_MODEL;

    BFL::NonLinearRevoluteMeasurementPdf* _meas_PDF;
    BFL::AnalyticMeasurementModelGaussianUncertainty* _meas_MODEL;

    BFL::ExtendedKalmanFilter* _ekf;

    void _initializeSystemModel();
    void _initializeMeasurementModel();
    void _initializeEKF();

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

    double _accumulated_rotation;
};

}

#endif /* REVOLUTEJOINTFILTER_H_ */