StaticEnvironmentFilter.h

Go to the documentation of this file.

/*
 * StaticEnvironmentFilter.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 STATIC_ENVIRONMENT_FILTER_H_
#define STATIC_ENVIRONMENT_FILTER_H_

#include "rb_tracker/RBFilter.h"
#include "omip_common/OMIPTypeDefs.h"
#include <pcl/registration/transformation_estimation_svd.h>
#include <tf/transform_listener.h>

namespace omip
{

typedef enum
{
    NO_CONSTRAINED = 0,
    NO_ROLL_PITCH = 1,
    NO_ROLL_PITCH_TZ = 2,
    NO_TRANSLATION = 3,
    NO_TRANSLATION_ROLL_YAW = 4,
    NO_ROTATION = 5,
    NO_MOTION = 6,
    ROBOT_XY_BASELINK_PLANE = 7
}
MotionConstraint;

class StaticEnvironmentFilter : public RBFilter
{
public:

    // Shared pointer type
    typedef boost::shared_ptr<StaticEnvironmentFilter> Ptr;

    StaticEnvironmentFilter(double loop_period_ns, FeaturesDataBase::Ptr feats_database, double environment_motion_threshold);

    virtual ~StaticEnvironmentFilter(){}

    virtual void predictState(double time_interval_ns);

    virtual void correctState();

    virtual void addSupportingFeature(Feature::Id supporting_feat_id);

    virtual void setMotionConstraint(int motion_constraint);

    virtual void estimateDeltaMotion(std::vector<Feature::Id>& supporting_features_ids, tf::Transform& previous_current_Tf);

    virtual void iterativeICP( pcl::PointCloud<pcl::PointXYZ>& previous_locations,
                                                 pcl::PointCloud<pcl::PointXYZ>& current_locations, tf::Transform& previous_current_Tf);

    virtual void constrainMotion();

    virtual void setComputationType(static_environment_tracker_t computation_type);

    virtual void Init();

private:
    // Minimum motion to consider that a feature moves
    double _environment_motion_threshold;
    int _motion_constraint;

    int _max_iterations;
    double _tf_epsilon_linear;
    double _tf_epsilon_angular;

    ros::NodeHandle _nh;
    tf::TransformListener _tf_listener;

    static_environment_tracker_t _computation_type;

    pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> _svdecomposer;
};
}

#endif /* STATIC_ENVIRONMENT_FILTER_H_ */