JointCombinedFilter.h

Go to the documentation of this file.

/*
 * JointCombinedFilter.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 JOINTCOMBINEDFILTER_H_
#define JOINTCOMBINEDFILTER_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/PoseWithCovariance.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>

#include "joint_tracker/JointFilter.h"
#include "omip_common/OMIPTypeDefs.h"

namespace omip
{

class JointCombinedFilter;
typedef boost::shared_ptr<JointCombinedFilter> JointCombinedFilterPtr;

typedef std::map<JointFilterType, JointFilterPtr> joint_filters_map;

class JointCombinedFilter
{
public:

    JointCombinedFilter();

  void setJointCombinedFilterId(JointCombinedFilterId new_joint_combined_filter_id);

  virtual ~JointCombinedFilter();

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

  JointCombinedFilter(const JointCombinedFilter &joint_combined_filter);

  virtual void predictState(double time_interval_ns);

  virtual void predictMeasurement();

  virtual void correctState();

  virtual void setMeasurement(joint_measurement_t acquired_measurement, const double& measurement_timestamp_ns);

  std::map<JointFilterType, Eigen::Twistd> getPredictedMeasurement();

  virtual void estimateJointFilterProbabilities();

  virtual void normalizeJointFilterProbabilities();

  virtual JointFilterPtr getMostProbableJointFilter();

  virtual JointFilterPtr getJointFilter(JointFilterType joint_type) const;

  virtual JointCombinedFilterId getJointCombinedFilterId() const;

  virtual void setJointLikelihoodDisconnected(double disconnected_joint_likelihood);

  virtual void setLoopPeriodNS(double loop_period_ns);

  virtual void setNumSamplesForLikelihoodEstimation(int likelihood_sample_num);

  virtual void setNormalizingTerm(double normalizing_term);

  virtual void setCovarianceDeltaMeasurementLinear(double sigma_delta_meas_uncertainty_linear);

  virtual void setCovarianceDeltaMeasurementAngular(double sigma_delta_meas_uncertainty_angular);

  virtual void initialize();

  virtual void setMaxTranslationRigid(double rig_max_translation);

  virtual void setMaxRotationRigid(double rig_max_rotation);

  virtual void setCovariancePrior(const JointFilterType& joint_type, double prior_cov_vel);

  virtual void setCovarianceAdditiveSystemNoisePhi(const JointFilterType& joint_type, double sys_noise_phi);

  virtual void setCovarianceAdditiveSystemNoiseTheta(const JointFilterType& joint_type, double sys_noise_theta);

  virtual void setCovarianceAdditiveSystemNoisePx(const JointFilterType& joint_type, double sys_noise_px);

  virtual void setCovarianceAdditiveSystemNoisePy(const JointFilterType& joint_type, double sys_noise_py);

  virtual void setCovarianceAdditiveSystemNoisePz(const JointFilterType& joint_type, double sys_noise_pz);

  virtual void setCovarianceAdditiveSystemNoiseJointState(const JointFilterType& joint_type, double sys_noise_js);

  virtual void setCovarianceAdditiveSystemNoiseJointVelocity(const JointFilterType& joint_type, double sys_noise_jv);

  virtual void setCovarianceMeasurementNoise(const JointFilterType& joint_type, double meas_noise);

  virtual void setMinRotationRevolute(const double& value);

  virtual void setMaxRadiusDistanceRevolute(const double& value);

  virtual void setInitialMeasurement(const joint_measurement_t &initial_measurement,
                                          const Eigen::Twistd& rrb_pose_at_srb_birth_in_sf, const Eigen::Twistd& srb_pose_at_srb_birth_in_sf);

protected:

  static JointCombinedFilterId _joint_id_generator;

  JointCombinedFilterId _joint_id;

  double _normalizing_term;

  // Contains an instance of each joint type
  joint_filters_map _joint_filters;

  // Contains the probabilities of each joint type
  std::map<JointFilterType, double> _joint_normalized_prediction_errors;

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

};

}

#endif /* JOINT_H_ */