source: pacpussensors/trunk/StereoVisionDisparity/ObstacleDetectionComponent.h@ 80

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

#ifndef OBSTACLEDETECTIONCOMPONENT_H
#define OBSTACLEDETECTIONCOMPONENT_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 "StereoVisionDisparityExp.h"

#include "../../StdDbtPlayerComponents/ImageBaseStructure.h"

#include <QThread>

namespace pacpus {

/** Class to provide the obstacle detection from a disparity map data */
class STEREOVISIONDISPARITY_API ObstacleDetectionComponent:     public QThread, 
                                                                                                                        public ComponentBase
{
public:
        //============================= DEFAULT ELEMENTS ===============================================
    ObstacleDetectionComponent(QString name);
    ~ObstacleDetectionComponent();

    void run();

    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, THREAD_ALIVE;


public:

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

private:

        double ANG_VARIATION, ANG_VARIATION2;

        int cam_width;                  // Image width
        int cam_height;                 // image height
        int cam_channels;               // number of channels
        double cam_ref_cx;              // cx camera divide by x image size
        double cam_ref_cy;              // cy camera divide by y image size
        double cam_ref_fx;              // fx camera divide by x image size
        double cam_ref_fy;              // fy camera divide by y image size
        double cam_baseline;    // distance between two cameras in meter
        bool showdebug;                 // Show frame acquired
        int min_disp;                   // Minimum disparity value to detect obstacles in the u/v-disp map
        int max_disp;                   // Maximum disparity value to detect obstacles in the u/v-disp map
        int min_disp_norm;              // Minimum disparity value to equalize the disp map 16
        int max_disp_norm;              // Maximum disparity value to equalize the disp map 16

        int destiny_roi_x;              // Destiny image roi x
        int destiny_roi_y;              // Destiny image roi y
        int destiny_roi_width;  // Destiny image roi width
        int destiny_roi_height; // Destiny image roi height

        bool use_roi;                   // If is to use roi

        int mMaxImageInputSize1;                // Size of the input image data in the memory
        int mMaxImageInputSize2;                // Size of the input image data in the memory
        int mMaxImageOutputSize;                // Size of the output image data in the memory

        TimestampedStructImage RefImageHeader;                  // Header for the reference image
        TimestampedStructImage DispInImageHeader;               // Header for the input disp image
        TimestampedStructImage Mask1ImageHeader;                // Header for the mask image 1
        TimestampedStructImage Mask2ImageHeader;                // Header for the mask image 2
        TimestampedStructImage DispOutImageHeader;              // Header for the output disp image

        void * ref_mem;
        void * dispin_mem;
        void * mask1_mem;
        void * mask2_mem;
        void * dispout_mem;

        size_t ref_mem_size;            // Image shared memory position size
        size_t dispin_mem_size;         // Image shared memory position size
        size_t mask1_mem_size;          // Image shared memory position size
        size_t mask2_mem_size;          // Image shared memory position size
        size_t dispout_mem_size;        // Image shared memory position size

        // Imput data
        ShMem * shmem_ref;                      // Shared memory control access to the image data
        ShMem * shmem_dispin;                   // Shared memory control access to the image data

        // Output data
        ShMem * shmem_mask1;            // Shared memory control access to the image data (free space mask)
        ShMem * shmem_mask2;            // Shared memory control access to the image data (obstacles mask)
        ShMem * shmem_dispout;          // Shared memory control access to the image data (disparity map 16)

        ShMem * shmem_dispMapNormalized;
        ShMem * shmem_result;

        cv::Mat CurrentReferenceFrame;  // Reference image
        cv::Mat CurrentDisparityMap16;  // Disparity Map

        cv::Mat CurrentSurfaceMask;             // Surface mask
        cv::Mat CurrentObstaclesMask;   // Obstacles mask

        // Function to calculate the U/V disparity map
        std::pair<cv::Mat, cv::Mat> CalcUVDisparityMap(cv::Mat disp_map);

        /*      CalcUVDisparityMapNorm
                Description:
                        Function to calculate the U/V disparity map from a disp map normalized
                Parameters:
                        disp_map16 = original disparity map 16
                        disp_map_norm = resulted disparity map normalized
                        min_d_norm = Minimum disparity value to equalize the disp map 16
                        max_d_norm = Maximum disparity value to equalize the disp map 16
        */
        void CalcUVDisparityMapNorm(cv::Mat disp_map16, cv::Mat &v_disp_map, cv::Mat &u_disp_map, cv::Mat &disp_map_norm, int min_d_norm, int max_d_norm);

        //Function to check the points order, making the second one with the highest y ever
        void CheckPoints(cv::Point &pt1, cv::Point &pt2);

        // Function to calculate the line slope of pt1 to pt2
        double Inclination(cv::Point pt1, cv::Point pt2);

        // Function to calculate the free space (v_disp_1) and obstacles (v_disp_2) masks from 
        // a red highlighted V-Disparity map
        std::pair<cv::Mat, cv::Mat> MaskSurface2(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, int min_d = 0, int max_d = 255, int value = 0);

        // Function to calculate the free space (v_disp_1) and obstacles (v_disp_2/u_disp) masks from 
        // a red highlighted V-Disparity map
        std::pair<cv::Mat, cv::Mat> MaskSurface3(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, cv::Mat u_disp, int min_d = 0, int max_d = 255, int value = 0);
        
        // Function to find the free space surface from a V-disparity map
        cv::Mat FindSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);

        /*      FindSurface2
                Description:
                        Function to find the free space surface from a V-disparity map, based on the frontal plane. 
                        Return the V-dysparity map with the red line representing the free surface.
                Parameters:
                        v_disp_map = Original V-disparity map
                        v_disp_map2 = Orignal V-disparity map less the surface detected
        */
        cv::Mat FindSurface2(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);

        // Function to find the free space surface from a V-disparity map with mean average
        cv::Mat FindAverageSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);

        // Function to find the near obstacles from a v-Disparity map
        cv::Mat FindNearObstacles(cv::Mat v_disp_map, int min_d = 0, int max_d = 255);

        // Function to find the near obstacles from the v/u-Disparity maps
        std::pair<cv::Mat, cv::Mat> FindNearObstaclesUV(cv::Mat v_disp_map, cv::Mat u_disp_map, int min_d = 0, int max_d = 255);

        /*      LinesFiltering
                Description:
                        Filter the detected lines related to the distance and angle between them.
                Parameters:
                        lines = line list (point 1 and 2)
        */
        std::vector<cv::Point> LinesFiltering(std::vector<cv::Vec4i> lines);

        /*      CalcMedian
                Description:
                        Calcule the median value of a vector.
                Parametros:
                        vector = vector with data to calculate the median
        */
        template<class A>
        A CalcMedian(std::vector<A> vetor) const;

        bool PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity);
};

}
#endif // OBSTACLEDETECTIONCOMPONENT