Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef OPENNI_LISTENER_H
00019 #define OPENNI_LISTENER_H
00020 #include "ros/ros.h"
00021
00022 #include <message_filters/subscriber.h>
00023 #include <message_filters/synchronizer.h>
00024 #include <message_filters/sync_policies/approximate_time.h>
00025 #include <sensor_msgs/Image.h>
00026 #include <sensor_msgs/CameraInfo.h>
00027 #include <sensor_msgs/PointCloud2.h>
00028 #include <nav_msgs/Odometry.h>
00029 #include <opencv2/core/core.hpp>
00030 #include <opencv2/features2d/features2d.hpp>
00031 #include "graph_manager.h"
00032 #include <qtconcurrentrun.h>
00033 #include <QImage>
00034 #include <QStringList>
00035 #include <rosbag/bag.h>
00036
00037
00039 namespace tf{
00040 class TransformListener;
00041 }
00043
00044
00045 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
00046 sensor_msgs::Image,
00047 sensor_msgs::PointCloud2> KinectSyncPolicy;
00048
00049 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
00050 sensor_msgs::PointCloud2> StereoSyncPolicy;
00051
00052
00053 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
00054 sensor_msgs::Image,
00055 sensor_msgs::CameraInfo> NoCloudSyncPolicy;
00056
00057
00062 template <class M>
00063 class BagSubscriber : public message_filters::SimpleFilter<M>
00064 {
00065 public:
00066 void newMessage(const boost::shared_ptr<M const> &msg) {
00067 this->signalMessage(msg);
00068 }
00069 };
00070
00072
00079 class OpenNIListener : public QObject {
00081 Q_OBJECT
00082 Q_SIGNALS:
00084 void newVisualImage(QImage);
00086 void newFeatureImage(QImage);
00087 void newFeatureFlowImage(QImage);
00089 void newDepthImage(QImage);
00090
00092 void setGUIInfo(QString message);
00093 void setGUIInfo2(QString message);
00095 void setGUIStatus(QString message);
00096 void bagFinished();
00097 void iamBusy(int id, const char* message, int max);
00098 void progress(int id, const char* message, int val);
00099
00100 public Q_SLOTS:
00102 void togglePause();
00104 void getOneFrame();
00105 void loadPCDFiles(QStringList);
00106 void loadBagFileFromGUI(QString);
00107
00108 public:
00110
00114 OpenNIListener(GraphManager* g_mgr);
00115
00117 ~OpenNIListener();
00119
00123 void kinectCallback (const sensor_msgs::ImageConstPtr& visual_img,
00124 const sensor_msgs::ImageConstPtr& depth_img,
00125 const sensor_msgs::PointCloud2ConstPtr& point_cloud);
00127 void noCloudCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,
00128 const sensor_msgs::ImageConstPtr& depth_img_msg,
00129 const sensor_msgs::CameraInfoConstPtr& cam_info_msg) ;
00131 void stereoCallback(const sensor_msgs::ImageConstPtr& visual_img_msg, const sensor_msgs::PointCloud2ConstPtr& point_cloud);
00133 void odomCallback(const nav_msgs::OdometryConstPtr& odom_msg);
00134 void pcdCallback(const sensor_msgs::ImageConstPtr visual_img_msg, pointcloud_type::Ptr point_cloud);
00135
00136 protected:
00138 void setupSubscribers();
00140 void loadBag(std::string filename);
00142 void loadBagFakeSubscriberSetup(const std::string& visua_tpc,
00143 const std::string& depth_tpc,
00144 const std::string& points_tpc,
00145 const std::string& cinfo_tpc,
00146 const std::string& odom_tpc,
00147 const std::string& tf_tpc);
00149 void processBagfile(std::string filename,
00150 const std::string& visua_tpc,
00151 const std::string& depth_tpc,
00152 const std::string& points_tpc,
00153 const std::string& cinfo_tpc,
00154 const std::string& odom_tpc,
00155 const std::string& tf_tpc);
00156
00158 void waitAndEvaluate(const std::string& filename);
00159
00161 void evaluation(std::string filename);
00162
00165 QImage cvMat2QImage(const cv::Mat& image, unsigned int idx);
00168 QImage cvMat2QImage(const cv::Mat& channel1, const cv::Mat& channel2, const cv::Mat& channel3, unsigned int idx);
00170 void retrieveTransformations(std_msgs::Header depth_header, Node* node_ptr);
00171 void visualize_images(cv::Mat visual_image, cv::Mat depth_image);
00172
00174 void callProcessing(cv::Mat gray_img, Node* node_ptr);
00176 void processNode(Node* new_node);
00177
00179 void cameraCallback(cv::Mat visual_img,
00180 pointcloud_type::Ptr point_cloud,
00181 cv::Mat depth_mono8_img);
00183 void noCloudCameraCallback(cv::Mat visual_img,
00184 cv::Mat depth,
00185 cv::Mat depth_mono8_img,
00186 std_msgs::Header depth_header,
00187 const sensor_msgs::CameraInfoConstPtr& cam_info);
00189 void loadPCDFilesAsync(QStringList);
00192 GraphManager* graph_mgr_;
00193
00194
00195 cv::Ptr<cv::FeatureDetector> detector_;
00196 cv::Ptr<cv::DescriptorExtractor> extractor_;
00197
00198 message_filters::Synchronizer<StereoSyncPolicy>* stereo_sync_;
00199 message_filters::Synchronizer<KinectSyncPolicy>* kinect_sync_;
00200 message_filters::Synchronizer<NoCloudSyncPolicy>* no_cloud_sync_;
00201 message_filters::Subscriber<sensor_msgs::Image> *visua_sub_;
00202 message_filters::Subscriber<sensor_msgs::Image> *depth_sub_;
00203 message_filters::Subscriber<sensor_msgs::CameraInfo> *cinfo_sub_;
00204 message_filters::Subscriber<sensor_msgs::PointCloud2> *cloud_sub_;
00205 message_filters::Subscriber<nav_msgs::Odometry> *odom_sub_;
00206
00207 BagSubscriber<sensor_msgs::Image>* rgb_img_sub_;
00208 BagSubscriber<sensor_msgs::Image>* depth_img_sub_;
00209 BagSubscriber<sensor_msgs::CameraInfo>* cam_info_sub_;
00210 BagSubscriber<sensor_msgs::PointCloud2>* pc_sub_;
00211
00212 cv::Mat depth_mono8_img_;
00214 cv::Mat visualization_depth_mono8_img_;
00216 cv::Mat visualization_img_;
00217 std::vector<cv::Mat> rgba_buffers_;
00218
00219 bool pause_;
00220 bool getOneFrame_;
00221 bool first_frame_;
00222 QFuture<void> future_;
00223 tf::TransformListener* tflistener_;
00224 tf::TransformBroadcaster tf_br_;
00225 ros::Publisher tf_pub_;
00226 int data_id_;
00227 int num_processed_;
00228 std::string image_encoding_;
00229 };
00230
00231 #endif