PointFeatureTracker.h

Go to the documentation of this file.

/*
 * PointFeatureTracker.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 REAL_FEATURE_TRACKER_H_
#define REAL_FEATURE_TRACKER_H_

#include "feature_tracker/FeatureTracker.h"

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <pcl/point_types.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/core/core.hpp>
//#include <opencv2/core/gpumat.hpp>
//#include <opencv2/gpu/gpu.hpp>
#include <camera_info_manager/camera_info_manager.h>

#include <iostream>
#include <fstream>

namespace omip
{

class PointFeatureTracker : public FeatureTracker
{
public:

    PointFeatureTracker(double loop_period_ns, bool using_pc = false, std::string ft_ns = std::string(""));

    virtual ~PointFeatureTracker();

    virtual void predictState(double time_interval_ns);

    virtual void predictMeasurement();

    virtual void correctState();

    virtual void addPredictedState(const ft_state_t& predicted_state, const double& predicted_state_timestamp_ns);

    virtual void setDynamicReconfigureValues(feature_tracker::FeatureTrackerDynReconfConfig &config);


    virtual void setFullRGBDPC(PointCloudPCL::ConstPtr full_rgb_pc);

    virtual void setOcclusionMaskImg(cv_bridge::CvImagePtr occ_mask_img);

    virtual void setCameraInfoMsg(const sensor_msgs::CameraInfo* camera_info);

    virtual ft_state_t getState() const;

    virtual cv_bridge::CvImagePtr getRGBImg();

    virtual cv_bridge::CvImagePtr getDepthImg();

    virtual cv_bridge::CvImagePtr getTrackedFeaturesImg();

    virtual cv_bridge::CvImagePtr getTrackedFeaturesWithPredictionMaskImg();

    virtual cv_bridge::CvImagePtr getDepthEdgesImg();

    virtual cv_bridge::CvImagePtr getPredictingMaskImg();

    virtual cv_bridge::CvImagePtr getTrackingMaskImg();

    virtual cv_bridge::CvImagePtr getDetectingMaskImg();

    virtual cv_bridge::CvImagePtr getPredictedAndLastFeaturesImg();

    virtual void setSelfOcclusionPositive(bool so_positive);


protected:

    void _ReadParameters();

    void _InitializeVariables();

    void _EstimateDetectionMask();

    void _DetectNewFeatures();

    void _TrackFeatures();

    void _ProcessDepthImg();

    void _ProcessRGBImg();

    void _ProcessOcclusionMaskImg();

    void _ProcessPredictedLocationsPC();

    int _Retrieve3DCoords(
            std::vector<cv::Point2f>& points_in_2d, std::vector<bool>& points_status,
            std::vector<cv::Point3d>& points_in_3d);

    int _Compare3DCoords();

    ros::NodeHandle _node_handle;
    sensor_msgs::CameraInfo::Ptr _camera_info_ptr;

    // PCL variables
    PointCloudPCL::ConstPtr _received_point_cloud;

    // OpenCV variables
    cv_bridge::CvImagePtr _depth_img_ptr, _rgb_img_ptr, _current_bw_img_ptr, _previous_bw_img_ptr, _occlusion_msk_img_ptr;
    cv_bridge::CvImagePtr _tracked_features_img, _tracked_features_with_pred_msk_img, _depth_edges_img, _predicting_mask_img;
    cv_bridge::CvImagePtr _tracking_mask_img, _detecting_mask_img, _predicted_and_previous_features_img;
    cv::Mat _predicted_feats_msk_mat, _depth_mat, _canny_edges_mat, _feats_detection_msk_mat, _feats_tracking_msk_mat;
    cv::Mat _erosion_element_detecting_mat, _erosion_element_tracking_mat, _occlusion_msk_mat, _aux1_mat, _previous_depth_mat, _depth_difference_mask;

    Eigen::Matrix<double, 3, 4> _image_plane_proj_mat_eigen;

    std::vector<std::vector<cv::Point2f> >  _predicted_measurements;
    std::vector<cv::Point2f> _previous_measurement, _corrected_measurement;
    std::vector<cv::Point3d> _previous_belief_state, _corrected_belief_state;
    std::vector<bool> _feat_status_v;
    std::vector<int> _previous_feat_ids_v, _current_feat_ids_v;
    std::vector<int> _received_prediction_ids;

    ros::Time _predicted_state_rcvd_time, _previous_time_of_motion_detection;
    double _predicted_state_rcvd_time_ns;

    // Flags
    bool _using_pc, _first_frame, _occlusion_msk_img_rcvd, _predicted_state_rcvd, _predicted_state_processed, _camera_info_msg_rcvd;
    bool _publishing_predicted_and_past_feats_img, _publishing_tracked_feats_img, _publishing_tracked_feats_with_pred_msk_img;
    bool _attention_to_motion, _use_motion_mask;

    // Parameters
    int _erosion_size_detecting, _erosion_size_tracking, _canny_kernel_size;
    int _canny_ratio, _number_of_features, _min_number_of_features, _frame_counter, _processing_factor, _min_area_size_pixels;
    double _max_allowed_depth, _min_allowed_depth, _canny_low_threshold, _max_interframe_jump, _sensor_fps, _min_time_to_detect_motion, _min_depth_difference, _min_feat_quality;

    Feature::Id _feature_id_counter;
    std::string _ft_ns;

    std::vector<std::vector<float> > _feat_quality;

    std::ofstream _features_file;

    bool _so_positive;
};
}

#endif /* REAL_FEATURE_TRACKER_H_ */