openni_listener.h

Go to the documentation of this file.

/* This file is part of RGBDSLAM.
 * 
 * RGBDSLAM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * RGBDSLAM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
 */


#ifndef OPENNI_LISTENER_H
#define OPENNI_LISTENER_H
#include "ros/ros.h"
//#include <pcl_tf/transforms.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "graph_manager.h"
#include <qtconcurrentrun.h>
#include <QImage> //for cvMat2QImage not listet here but defined in cpp file
#include <QStringList> 
#include <rosbag/bag.h>

//forward-declare to avoid including tf
namespace tf{
  class TransformListener;
}

//The policy merges kinect messages with approximately equal timestamp into one callback 
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, 
                                                        sensor_msgs::Image, 
                                                        sensor_msgs::PointCloud2> KinectSyncPolicy;

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
                                                        sensor_msgs::PointCloud2> StereoSyncPolicy;

//The policy merges kinect messages with approximately equal timestamp into one callback 
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, 
                                                        sensor_msgs::Image,
                                                        sensor_msgs::CameraInfo> NoCloudSyncPolicy;


template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg) { 
    this->signalMessage(msg); //"this->" is required as of ros groovy
  }
};


class OpenNIListener : public QObject {
  Q_OBJECT
  Q_SIGNALS:
    void newVisualImage(QImage);
    void newFeatureImage(QImage);
    void newFeatureFlowImage(QImage);
    void newDepthImage(QImage);
    //void pauseStatus(bool is_paused);
    void setGUIInfo(QString message);
    void setGUIInfo2(QString message);
    void setGUIStatus(QString message);
    void bagFinished();
    void iamBusy(int id, const char* message, int max);
    void progress(int id, const char* message, int val);

  public Q_SLOTS:
    void togglePause();
    void getOneFrame();
    void loadPCDFiles(QStringList);
    void loadBagFileFromGUI(QString);

  public:

    OpenNIListener(GraphManager* g_mgr);

    ~OpenNIListener();

    void kinectCallback (const sensor_msgs::ImageConstPtr& visual_img,
                         const sensor_msgs::ImageConstPtr& depth_img, 
                         const sensor_msgs::PointCloud2ConstPtr& point_cloud);
    void noCloudCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,
                          const sensor_msgs::ImageConstPtr& depth_img_msg,
                          const sensor_msgs::CameraInfoConstPtr& cam_info_msg) ;
    void stereoCallback(const sensor_msgs::ImageConstPtr& visual_img_msg, const sensor_msgs::PointCloud2ConstPtr& point_cloud);
    void odomCallback(const nav_msgs::OdometryConstPtr& odom_msg);
    void pcdCallback(const sensor_msgs::ImageConstPtr visual_img_msg, pointcloud_type::Ptr point_cloud);

  protected:
    void setupSubscribers();
    void loadBag(std::string filename);
    void loadBagFakeSubscriberSetup(const std::string& visua_tpc,
                                    const std::string& depth_tpc,
                                    const std::string& points_tpc,
                                    const std::string& cinfo_tpc,
                                    const std::string& odom_tpc,
                                    const std::string& tf_tpc);
    void processBagfile(std::string filename,
                        const std::string& visua_tpc,
                        const std::string& depth_tpc,
                        const std::string& points_tpc,
                        const std::string& cinfo_tpc,
                        const std::string& odom_tpc,
                        const std::string& tf_tpc);

    void waitAndEvaluate(const std::string& filename);

    void evaluation(std::string filename);

    QImage cvMat2QImage(const cv::Mat& image, unsigned int idx); 
    QImage cvMat2QImage(const cv::Mat& channel1, const cv::Mat& channel2, const cv::Mat& channel3, unsigned int idx);
    void retrieveTransformations(std_msgs::Header depth_header, Node* node_ptr);
    void visualize_images(cv::Mat visual_image, cv::Mat depth_image);

    void callProcessing(cv::Mat gray_img, Node* node_ptr);
    void processNode(Node* new_node);

    void cameraCallback(cv::Mat visual_img, 
                        pointcloud_type::Ptr point_cloud, 
                        cv::Mat depth_mono8_img);
    void noCloudCameraCallback(cv::Mat visual_img, 
                               cv::Mat depth, 
                               cv::Mat depth_mono8_img,
                               std_msgs::Header depth_header,
                               const sensor_msgs::CameraInfoConstPtr& cam_info);
    void loadPCDFilesAsync(QStringList);
    GraphManager* graph_mgr_;
    
    //Variables
    cv::Ptr<cv::FeatureDetector> detector_;
    cv::Ptr<cv::DescriptorExtractor> extractor_;

    message_filters::Synchronizer<StereoSyncPolicy>* stereo_sync_;
    message_filters::Synchronizer<KinectSyncPolicy>* kinect_sync_;
    message_filters::Synchronizer<NoCloudSyncPolicy>* no_cloud_sync_;
    message_filters::Subscriber<sensor_msgs::Image> *visua_sub_;      
    message_filters::Subscriber<sensor_msgs::Image> *depth_sub_;      
    message_filters::Subscriber<sensor_msgs::CameraInfo> *cinfo_sub_;      
    message_filters::Subscriber<sensor_msgs::PointCloud2> *cloud_sub_;
    message_filters::Subscriber<nav_msgs::Odometry> *odom_sub_;

    BagSubscriber<sensor_msgs::Image>* rgb_img_sub_;
    BagSubscriber<sensor_msgs::Image>* depth_img_sub_;
    BagSubscriber<sensor_msgs::CameraInfo>* cam_info_sub_;
    BagSubscriber<sensor_msgs::PointCloud2>* pc_sub_;

    cv::Mat depth_mono8_img_;
    cv::Mat visualization_depth_mono8_img_;
    cv::Mat visualization_img_;
    std::vector<cv::Mat> rgba_buffers_;
    
    bool pause_;
    bool getOneFrame_;
    bool first_frame_;
    QFuture<void> future_;
    tf::TransformListener* tflistener_; 
    tf::TransformBroadcaster tf_br_;
    ros::Publisher tf_pub_;
    int data_id_;
    int num_processed_;
    std::string image_encoding_;
};

#endif