depth.hpp

Go to the documentation of this file.

#ifndef _THERMALVIS_DEPTH_H_
#define _THERMALVIS_DEPTH_H_

#include "time.h"

#include <stereo_msgs/DisparityImage.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>

#include <boost/thread/thread.hpp>

#include <ros/ros.h>

// #include <image_geometry/image_geometry.h>

// #include "/opt/ros/electric/stacks/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h"
#include <image_geometry/stereo_camera_model.h>

namespace enc = sensor_msgs::image_encodings;

//#include "../../shared-src/super.h"
#include "ros_resources.hpp"
#include "general_resources.hpp"
#include "opencv_resources.hpp"
#include "camera.hpp"
#include "tools.hpp"
#include "improc.hpp"
#include "stereo.hpp"
// #include "../../shared-src/video.h"
// #include "../../shared-src/reconstruction.h"

#include <dynamic_reconfigure/server.h>
#include "depthConfig.h"

#define MAXIMUM_FRAMES_TO_STORE 100

const char __PROGRAM__[] = "DEPTH";

typedef dynamic_reconfigure::Server < thermalvis::depthConfig > Server;

using namespace std;
using namespace cv;

struct stereoData {
        string video_stream;
        cameraParameters cameraData1, cameraData2;
        
        double alpha;
        bool autoAlpha;
        
        string read_addr;
        string extrinsics;
        
        bool debugMode;
        double maxTimeDiff;
        
        // May also need some extrinsic data, extracted from the streams...
        
        bool obtainStartingData(ros::NodeHandle& nh);   
        
};

class stereoDepthNode {
private:

        ros::Timer timer;
        
        bool alphaChanged;
        
        //sensor_msgs::Image disp_image;
        stereo_msgs::DisparityImage disp_object;
        sensor_msgs::PointCloud2 colouredCloud;
        
        image_geometry::StereoCameraModel stereo_model;
        
        sensor_msgs::CameraInfo cam_info_1, cam_info_2;
        
        Mat t;
        Mat R0, R1;
        Mat P0, P1;
        Mat F, Q1, Q2;
        
        unsigned int checkIndex_1, checkIndex_2;
        // unsigned int duplicateFlags_1[MAXIMUM_FRAMES_TO_STORE], duplicateFlags_2[MAXIMUM_FRAMES_TO_STORE];
        ros::Time time_buffer_1[MAXIMUM_FRAMES_TO_STORE], time_buffer_2[MAXIMUM_FRAMES_TO_STORE];
        vector<unsigned int> validPairs[2];
        unsigned int pairsProcessed;
        
        ros::Publisher disparityPublisher;

        stereoData configData;
        
        sensor_msgs::CameraInfo debug_camera_info;
        
        char cam_pub_name_1[256], cam_pub_name_2[256];
        sensor_msgs::Image msg_1, msg_2;
        sensor_msgs::CameraInfo camera_info_1, camera_info_2;
        image_transport::CameraPublisher depth_pub, cam_pub_1, cam_pub_2;
        
        image_transport::CameraSubscriber camera_sub_1, camera_sub_2;
        
        Mat disp16;
        
        cv_bridge::CvImagePtr cv_ptr_1;
        cv_bridge::CvImagePtr cv_ptr_2;
        
        Mat rawImageBuffer_1[MAXIMUM_FRAMES_TO_STORE], rawImageBuffer_2[MAXIMUM_FRAMES_TO_STORE];
        Mat newImage, grayImage, realImage, mappedImage, lastImage_1, lastImage_2, olderImage_1, olderImage_2, normalizedMat;

        Mat map11, map12, map21, map22;
        
        Rect roi1, roi2;

        struct timeval cycle_timer;
        double elapsedTime;

        unsigned int frameCount, frameCount_1, frameCount_2;

        char depth_pub_name[256];
        ros::NodeHandle private_node_handle;
        
        string nodeName;
        
        Mat displayImage, drawImage;
        
        Mat grayImageBuffer_1[MAXIMUM_FRAMES_TO_STORE], grayImageBuffer_2[MAXIMUM_FRAMES_TO_STORE];
        
        Mat blurredImage;
        
        bool infoProcessed_1;
        bool infoProcessed_2;
        
        bool firstImProcessed_1, firstImProcessed_2, firstPairProcessed;
        
        StereoBM bm;
        StereoSGBM sgbm;
        Mat disp, disp8, dispFloat, disparityImage;
        
        int SADWindowSize;
        int numberOfDisparities;
        
        bool firstPair;
        
        boost::shared_mutex _access;
        
        dynamic_reconfigure::Server<thermalvis::depthConfig> server;
        dynamic_reconfigure::Server<thermalvis::depthConfig>::CallbackType f;
        
public:
        stereoDepthNode(ros::NodeHandle& nh, stereoData startupData);
        
        void timed_loop(const ros::TimerEvent& event);
        
        void handle_image_1(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
        void handle_image_2(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
    
    void publishMaps();
        
        void assignDepthCameraInfo();
        
        void updatePairs();
        void updateMaps();
        
        void serverCallback(thermalvis::depthConfig &config, uint32_t level);

};

#endif