PrismaticJointFilter.h

Go to the documentation of this file.

/*
 * PrismaticJointFilter.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 PRISMATICJOINTFILTER_H_
#define PRISMATICJOINTFILTER_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/NonLinearPrismaticMeasurementPdf.h"

namespace omip
{

class PrismaticJointFilter;
typedef boost::shared_ptr<PrismaticJointFilter> PrismaticJointFilterPtr;

class PrismaticJointFilter : public JointFilter
{
public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  PrismaticJointFilter();

  virtual ~PrismaticJointFilter();

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

  PrismaticJointFilter(const PrismaticJointFilter &prismatic_joint);

  virtual void predictState(double time_interval_ns);

  virtual void predictMeasurement();

  virtual void correctState();

  virtual void estimateMeasurementHistoryLikelihood();
  virtual geometry_msgs::TwistWithCovariance getPredictedSRBPoseWithCovInSensorFrame();

  virtual geometry_msgs::TwistWithCovariance getPredictedSRBDeltaPoseWithCovInSensorFrame();

  virtual geometry_msgs::TwistWithCovariance getPredictedSRBVelocityWithCovInSensorFrame();

  virtual void estimateUnnormalizedModelProbability() ;

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

  virtual JointFilterType getJointFilterType() const;

  virtual std::string getJointFilterTypeStr() const;

  virtual void initialize();

  virtual void setCovarianceDeltaMeasurementLinear(double delta_meas);

protected:

  double _sigma_delta_meas_uncertainty_linear;

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

  BFL::NonLinearPrismaticMeasurementPdf* _meas_PDF;
  BFL::AnalyticMeasurementModelGaussianUncertainty* _meas_MODEL;

  BFL::ExtendedKalmanFilter* _ekf;

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

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

};

}

#endif /* PRISMATICJOINTFILTER_H_ */