source: pacpussensors/trunk/StereoVisionDisparity/CameraObstacleGridComponent.h@ 79

/*********************************************************************
//  created:    2013/06/27 - 18:40
//  filename:   CameraObstacleGridComponent.h
//
//  author:     Danilo Alves de Lima
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id: $
//
//  purpose:    
*********************************************************************/

#ifndef CAMERAOBSTACLEGRIDCOMPONENT_H
#define CAMERAOBSTACLEGRIDCOMPONENT_H

#include <fstream>
#include <qcoreevent.h>
#include <qthread.h>
#include <string>

#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include "Pacpus/kernel/ComponentBase.h"
#include "Pacpus/kernel/DbiteFile.h"
#include "Pacpus/PacpusTools/ShMem.h"
//#include "Pacpus/PacpusTools/UDPConnection.h"
#include "StereoVisionDisparityExp.h"
#include "SensorsApplicationStructures.h"

#include <QThread>
#include <QMutex>
#include <QFile.h>
#include <QTextStream>

namespace pacpus {

/** Class to convert the image classification masks of free space and obstacles in a 2D grid */
class STEREOVISIONDISPARITY_API CameraObstacleGridComponent:    public QThread, 
                                                                                                                        public ComponentBase
{

        Q_OBJECT

public:
        //============================= DEFAULT ELEMENTS ===============================================
    CameraObstacleGridComponent(QString name);
    ~CameraObstacleGridComponent();

        // Run for stereo camera data
    void run();

        // Run for mono camera data 
        void run2();

    virtual void stopActivity(); /*!< to stop the processing thread */
    virtual void startActivity(); /*!< to start the processing thread */
    virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
        //==============================================================================================
                
protected:

        // Indicates that thread is running
        bool is_running;

public:

        /** 
        * Initialize default values
        */
        void InitDefault();

private:

        int run_stereo;         // Select the work mode (stereo camera data or mono camera data)

        bool use_udpconnection; // Uses the udp connection to send the obstacles data

        //QHostAddress destiny_IP;              // Destiny IP
    //quint16 communication_Port;               // Communication port

        int cam_width;                  // Image width
        int cam_height;                 // image height
        double cam_fov;                 // Field of view of the camera
        int cam_valid_col0;             // Initial valid column 
        int cam_valid_row0;             // Initial valid row
        int cam_valid_cols;             // Valid columns 
        int cam_valid_rows;             // Valid rows
        double cam_fx;                  // Focus in x (pixels)
        double cam_fy;                  // Focus in y (pixels)
        double cam_cx;                  // Center in x (pixels)
        double cam_cy;                  // Center in y (pixels)
        double cam_baseline;    // Baseline (meters)
        double cam_tilt_angle;  // Inclination angle of the camera
        double cam_h;                   // Camera height
        double CorrAccuracy;    // Estimated camera correlation accuracy
        bool showdebug;                 // Show frame acquired
                
        int mMaxImageInputSize1;                // Size of the input image data in the memory
        int mMaxImageInputSize2;                // Size of the input image data in the memory

        int mMaxOutputSize1;                    // Size of the out data in the memory

        TimestampedStructImage Mask1ImageHeader;                // Header for the mask image 1
        TimestampedStructImage Mask2ImageHeader;                // Header for the mask image 2
        TimestampedStructImage DispImageHeader;                 // Header for the disp image

        //------------------------- Camera occupancy grid projection -----------------------------
        int data_type;                                                                  // 0 = camera data to laser format | 1 = camera data to occupancy grid format
        TimestampedCameraOccData Camdata2Laser;                 // Camera obstacle data                 ( for projection like a laser )

        TimestampedSensorOccData CamOccGrid;                    // Occupancy grid mapping from the stereo image         ( for complete projection of the image data )
        
        double D_MAX_CAM_GRID;          // Sensor max valid distance in the grid
        double sub_div;                         // Grid subdivision

        double free_area_guess;         // Frontal projected area expected to be a road surface (frontal road surface uncouvered by camera FOV) 
        //-----------------------------------------------------------------------------------------

        void* mask1_mem;
        void* mask2_mem;
        void* disp_mem;

        size_t mask1_mem_size;          // Image shared memory position size
        size_t mask2_mem_size;          // Image shared memory position size
        size_t disp_mem_size;           // Image shared memory position size

        // Imput data
        ShMem * shmem_mask1;                    // Shared memory control access to the image data
        ShMem * shmem_mask2;                    // Shared memory control access to the image data
        ShMem * shmem_disp16;                   // Shared memory control access to the image data

        // Output data
        ShMem * shmem_obst;                             // Shared memory control access to the image data

        // Output data via UDP
//      UDPConnection* udp_con;         

        cv::Mat CurrentMask1Frame;              // Mask1 = free space image
        cv::Mat CurrentMask2Frame;              // Mask2 = obstacles image
        cv::Mat CurrentDisparityMap16;  // Disparity Map 16 bits

        cv::Mat CurrentXMatrix;                 // Matrix with the x mapping (robot frame)
        cv::Mat CurrentYMatrix;                 // Matrix with the y mapping (robot frame)
        
        // Function to create the obstacle's grid by the obstacles mask image. Similar to a laser.
        void CreateCamera2LaserGrid(cv::Mat mask_obstacles, cv::Mat disp_map16, double* radius_data, double* angle_data);

        // Function to create the obstacle's grid by the road and obstacles mask images.
        void CreateCameraGrid(cv::Mat mask_free, cv::Mat mask_obstacles, cv::Mat disp_map16, sensor_occ_data &occ_data);

        /*      CreateMonoCameraGrid
                Description:
                        Function to create the road surface grid by the mono camera segmented image.
                Parameters:
                        mask_free = road surface mask
                        occ_data = occupancy grid
                        max_dist = max distance to be considerated
                        img_step = step between the image rows and cols
        */ 
        void CreateMonoCameraGrid(cv::Mat mask_free, sensor_occ_data &occ_data, double max_dist, int img_step);

        /*      CreateProjectionMatrix
                Description:
                        Function to create the matrixes with the image projection in the 
                        robot plane.
                Parameters:
                        max_dist = max distance to be considerated
        */ 
        void CreateProjectionMatrix(double max_dist);

        /*      PointTriangulate
                Description:
                        Calculate the point triangulation in the world
                Parameters:
                        row,col = row and column in the image
                        x,y,z = world coordinates
                        disparity = disparity value
        */
        bool PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity);

        /*      Calc3DPointProjection
                Description:
                        Calculate the 3D point projection in a world plane
                Parameters:
                        row, col = row and column in the image
                        x_r, y_r= world coordinates (in the robot frame)
        */
        void Calc3DPointProjection(int row, int col, double &x_r, double &y_r);

        // Draw the camera obstacle grid
        cv::Mat DrawGrid(double* radius_data, double* angle_data);

        // Draw the camera occupancy grid
        cv::Mat DrawGrid(sensor_occ_data &occ_data);

        bool recording, THREAD_ALIVE;

        double maxDistToBrake;
        QFile saveFile_;
        QTextStream flux_;

Q_SIGNALS:
        void smallestDistance();
};

}
#endif // CAMERAOBSTACLEGRIDCOMPONENT