/* 
 * File:   Stereo.cpp
 * Author: carlos
 * 
 * Created on 5 de diciembre de 2012, 10:34
 */

#include "Stereo.h"
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/angles.h>
#include <pcl/common/distances.h>
#include <CpuTime.h>

using namespace cv;

Stereo::Stereo() {

}

Stereo::Stereo(const Stereo& orig) {
}

Stereo::~Stereo() {
}

void Stereo::showCurvImg(pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr curv,
                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyz,
                         cv::Mat img, float threshold, int r, int g, int b) {

    cv::Mat imgColor;
    cvtColor(img, imgColor, CV_GRAY2RGB);
    printf("----CurvatureImg----\nCurvatures = %d\nXYZ = %d\n", curv->points.size(), xyz->points.size());


    int out = 0;
    for (int i = 0; i < curv->points.size(); i++) {

        pcl::PrincipalCurvatures ptC = curv->points.at(i);

        if (fabs(ptC.principal_curvature_z) >= threshold) {
            pcl::PointXYZRGB pt = xyz->points.at(i);
            float x = pt.x;
            float y = pt.y;
            float z = pt.z;
            int uh, vh;
            float xc, yc, zc;
            fromCloud_to_cam3D(x, y, z, &xc, &yc, &zc);
            World2Img(&m_camLlorca, xc, yc, zc, &uh, &vh);

            xyz->points.at(i).r = r;
            xyz->points.at(i).g = g;
            xyz->points.at(i).b = b;

            Point center;
            center.x = uh;
            center.y = vh;
            int radius = 1;
            circle(imgColor, center, radius, CV_RGB(r, g, b), -1);
//            if (uh < 0 || uh > imgColor.cols || vh < 0 || vh > imgColor.rows) {
//                out++;
//                printf("[%d] point %d out of image(%d , %d)!!(U %d)(V %d)\n", out, i, imgColor.cols, imgColor.rows, uh, vh);
//            }
        }
    }
    imshow(string("CurvImage"), imgColor);

}

int Stereo::initKittiStereo(string calibFile, string leftDir, string rightDir, string pcdDir) {
    int ret = 0;

    //read calib file
    t_CalibParams calibParams;
    this->readKittiStereoCalibFiles(calibFile, &calibParams);
    //compute R, P and Q
    this->m_calib_params = calibParams;
    this->m_k00 = cv::Mat(3, 3, CV_64FC1, this->m_calib_params.camera[0].matrix);
    this->m_d00 = cv::Mat(1, 5, CV_64FC1, this->m_calib_params.camera[0].distortion);
    this->m_k01 = cv::Mat(3, 3, CV_64FC1, this->m_calib_params.camera[1].matrix);
    this->m_d01 = cv::Mat(1, 5, CV_64FC1, this->m_calib_params.camera[1].distortion);
    this->m_r01 = cv::Mat(3, 3, CV_64FC1, this->m_calib_params.rot_matrix);
    this->m_t01 = cv::Mat(3, 1, CV_64FC1, this->m_calib_params.trans_vector);
    this->m_rRect00 = cv::Mat(3, 3, CV_64FC1);
    this->m_rRect01 = cv::Mat(3, 3, CV_64FC1);
    this->m_pRect00 = cv::Mat(3, 4, CV_64FC1);
    this->m_pRect01 = cv::Mat(3, 4, CV_64FC1);
    this->m_q = cv::Mat(4, 4, CV_64FC1);
    cv::stereoRectify(m_k00, m_d00, m_k01, m_d01, Size(m_frameWidth, m_frameHeight), m_r01, m_t01, m_rRect00, m_rRect01, m_pRect00, m_pRect01, m_q, CV_CALIB_ZERO_DISPARITY);

    //    printf("m_rRect00: ");
    //    for (int r = 0; r < m_rRect00.rows; r++) {
    //        for (int c = 0; c < m_rRect00.cols; c++) {
    //            printf("%le ",m_rRect00.at<double>(r,c));
    //        }        
    //    }
    //    printf("\n");
    //    printf("m_rRect01: ");
    //    for (int r = 0; r < m_rRect01.rows; r++) {
    //        for (int c = 0; c < m_rRect01.cols; c++) {
    //            printf("%le ",m_rRect01.at<double>(r,c));
    //        }        
    //    }
    //    printf("\n");
    //    printf("m_pRect00: ");
    //    for (int r = 0; r < m_pRect00.rows; r++) {
    //        for (int c = 0; c < m_pRect00.cols; c++) {
    //            printf("%le ",m_pRect00.at<double>(r,c));
    //        }        
    //    }
    //    printf("\n");
    //    printf("m_pRect01: ");
    //    for (int r = 0; r < m_pRect01.rows; r++) {
    //        for (int c = 0; c < m_pRect01.cols; c++) {
    //            printf("%le ",m_pRect01.at<double>(r,c));
    //        }        
    //    }
    //    printf("\n");


    //    printf("%lf %lf %lf");
    //cv::stereoRectify(_M1, _D1, _M2, _D2, cv::Size(calib_params->camera[1].img_size[0], calib_params->camera[1].img_size[1]), _R, _T, _R1, _R2, _P1, _P2, map_matrices->mq, CV_CALIB_ZERO_DISPARITY);


    //set directories
    this->m_kittiLeft = leftDir;
    this->m_kittiRight = rightDir;
    this->m_kittiPCD = pcdDir;

    //    m_frameWidth = width;
    //    m_frameHeight = height;
    //    m_numChannels = numChannels;
    //
    //    fseeko64(m_leftFile, (off64_t) 0, SEEK_END);
    //    off64_t posEnd = ftello64(m_leftFile);
    //    m_videoLength = posEnd / (m_frameWidth * m_frameHeight * numChannels);
    //
    //    ret = InitStereo(&m_map_matrices, &m_calib_params, &m_bm_state, calibPath);
    //    m_mask = imread(mask, CV_LOAD_IMAGE_GRAYSCALE);
    //    if (m_mask.data == NULL) {
    //        printf("Error reading mask file: %s\n", mask.data());
    //    }
    //
    //    switch (numChannels) {
    //        case 1:
    //            m_images_stereo.left_image = Mat(m_frameHeight, m_frameWidth, CV_8UC1);
    //            m_images_stereo.right_image = Mat(m_frameHeight, m_frameWidth, CV_8UC1);

    return ret;
}

int Stereo::openStereoFile(string leftPath, string rightPath, string calibPath, string mask, int width, int height, int numChannels) {

    int ret = 0;
    //    string aaa = string("/media/TESIS_CARLOS/VideosNacho/11MayShutterAuto/video_cero_0005.raw");
    //    string bbb = string("/media/TESIS_CARLOS/VideosNacho/11MayShutterAuto/video_uno_0005.raw");
    //    m_leftFile = fopen64(aaa.data(),"r");
    //    m_rightFile = fopen64(bbb.data(),"r");

    m_leftFile = fopen64(leftPath.data(), "r");
    m_rightFile = fopen64(rightPath.data(), "r");

    if (m_leftFile == NULL || m_rightFile == NULL) {
        printf("Error opening: \n%s%s", leftPath.data(), rightPath.data());
        return -1;
    }

    m_frameWidth = width;
    m_frameHeight = height;
    m_numChannels = numChannels;

    fseeko64(m_leftFile, (off64_t) 0, SEEK_END);
    off64_t posEnd = ftello64(m_leftFile);
    m_videoLength = posEnd / (m_frameWidth * m_frameHeight * numChannels);

    ret = InitStereo(&m_map_matrices, &m_calib_params, &m_bm_state, calibPath);
    m_mask = imread(mask, CV_LOAD_IMAGE_GRAYSCALE);
    if (m_mask.data == NULL) {
        printf("Error reading mask file: %s\n", mask.data());
    }

    switch (numChannels) {
        case 1:
            m_images_stereo.left_image = Mat(m_frameHeight, m_frameWidth, CV_8UC1);
            m_images_stereo.right_image = Mat(m_frameHeight, m_frameWidth, CV_8UC1);
            break;
        case 3:
            m_images_stereo.left_image = Mat(m_frameHeight, m_frameWidth, CV_8UC3);
            m_images_stereo.right_image = Mat(m_frameHeight, m_frameWidth, CV_8UC3);
            break;
    }


    return ret;
}

int Stereo::setMaskFile(string maskFile) {

    if (m_mask.data == NULL)
        m_mask = Mat(m_frameHeight, m_frameWidth, CV_8UC1);
    m_mask = imread(maskFile, CV_LOAD_IMAGE_GRAYSCALE);

    int ret = -(m_mask.data == NULL);
    return ret;
}

int Stereo::getVideoLength() {
    return m_videoLength;
}

int Stereo::readFrame(int n) {
    int ret = -1;

    if (n >= 0 && n < m_videoLength) {
        //start reading video
        fseeko64(m_leftFile, (off64_t) n * m_frameWidth * m_frameHeight*m_numChannels, SEEK_SET);
        fseeko64(m_rightFile, (off64_t) n * m_frameWidth * m_frameHeight*m_numChannels, SEEK_SET);

        if (fread(m_images_stereo.left_image.data, sizeof (char), m_frameWidth * m_frameHeight * m_numChannels, m_leftFile) > 0 &&
            fread(m_images_stereo.right_image.data, sizeof (char), m_frameWidth * m_frameHeight * m_numChannels, m_rightFile) > 0) {
            ret = 0;
            m_currFrame = n;
        }
    }

    return ret;
}

int Stereo::showMask() {
    cv::imshow("mask", m_mask);
    return 0;
}

int Stereo::showLeft() {
    cv::imshow("left", m_images_stereo.left_image);
    return 0;
}

int Stereo::showRight() {
    cv::imshow("right", m_images_stereo.right_image);
    return 0;
}

int Stereo::showUndistLeft() {
    cv::imshow("undist_left", m_images_stereo.undist_left_image);
    return 0;
}

int Stereo::showUndistRight() {
    cv::imshow("undist_right", m_images_stereo.undist_right_image);
    return 0;
}

int Stereo::processDisparity() {
    exampleOpencv(&m_images_stereo, &m_map_matrices, m_bm_state, m_mask);
    GetDisparityMapStereo(&m_map_matrices, &m_bm_state, &m_images_stereo);
    return 0;
}

int Stereo::showDisparity() {
    cv::imshow("disparity", m_images_stereo.vdisp_image);
    return 0;
}

int Stereo::getPoint_3dImage(int u, int v, Stereo::t_Point* pt) {
    Stereo::t_Point point;
    int ret = -1;

    uchar* rgb_ptr = m_images_stereo.undist_left_image.ptr<uchar>(v);
    double* xyz_ptr = m_images_stereo.recons3D.ptr<double>(v);

    uchar vd = m_images_stereo.vdisp_image.at<uchar>(v, u);
    if (vd != 0) {
        ret = 0;

        // Get RGB information.
        uchar pr, pg, pb;
        pb = rgb_ptr[m_images_stereo.undist_left_image.channels() * u];
        pg = rgb_ptr[m_images_stereo.undist_left_image.channels() * u + 1];
        pr = rgb_ptr[m_images_stereo.undist_left_image.channels() * u + 2];

        double px, py, pz, pw;
        //        px = xyz_ptr[m_images_stereo.recons3D.channels() * u];
        //        py = xyz_ptr[m_images_stereo.recons3D.channels() * u + 1];
        //        pz = xyz_ptr[m_images_stereo.recons3D.channels() * u + 2];
        px = m_images_stereo.recons3D.at<Vec3f>(v, u)[0];
        py = m_images_stereo.recons3D.at<Vec3f>(v, u)[1];
        pz = m_images_stereo.recons3D.at<Vec3f>(v, u)[2];



        // Insert information into point cloud structure.      
        point.x = -px / 1000.0;
        point.y = py / 1000.0;
        point.z = pz / 1000.0;
        //        point.x = -px;
        //        point.y = py;
        //        point.z = pz;

        double xx, yy, zz;
        xx = -point.z;
        yy = -point.x;
        zz = point.y;
        point.x = xx;
        point.y = yy;
        point.z = zz; //sumar altura camara

        point.r = pr;
        point.g = pg;
        point.b = pb;
        //        point.r = 255;
        //        point.g = 255;
        //        point.b = 255;

        point.u = u;
        point.v = v;

        //        printf("(u,v)(%d,%d) = %.02lf %.02lf %.02lf (%d %d %d)\n",u,v,xx,yy,zz,pr,pg,pb);

        *pt = point;

    }



    return ret;
}

int Stereo::get3dPoint(int u, int v, Stereo::t_Point *pt) {
    Stereo::t_Point point;
    int ret = -1;

    uchar* rgb_ptr = m_images_stereo.undist_left_image.ptr<uchar>(v);

    uchar d = m_images_stereo.disp_image.at<uchar>(v, u);
    uchar vd = m_images_stereo.vdisp_image.at<uchar>(v, u); //BUENA
    if (vd != 0) {
        ret = 0;

        // Get RGB information.
        uchar pr, pg, pb;
        pb = rgb_ptr[m_images_stereo.undist_left_image.channels() * u];
        pg = rgb_ptr[m_images_stereo.undist_left_image.channels() * u + 1];
        pr = rgb_ptr[m_images_stereo.undist_left_image.channels() * u + 2];

        double px, py, pz, pw;
        pw = (static_cast<double> (vd)) * m_map_matrices.mq.at<double>(3, 2) + m_map_matrices.mq.at<double>(3, 3); //BUENA
        //        pw = -1.0 * (static_cast<double> (vd)) * m_map_matrices.mq.at<double>(3, 2) + m_map_matrices.mq.at<double>(3, 3);
        px = static_cast<double> (u) + m_map_matrices.mq.at<double>(0, 3);
        py = static_cast<double> (v) + m_map_matrices.mq.at<double>(1, 3);
        pz = m_map_matrices.mq.at<double>(2, 3);

        px = px / pw;
        py = py / pw;
        pz = pz / pw;

        // Insert information into point cloud structure.      
        point.x = -px / 1000.0;
        point.y = py / 1000.0;
        point.z = pz / 1000.0;
        //        point.x = -px;
        //        point.y = py;
        //        point.z = pz;

        double xx, yy, zz;
        xx = -point.z;
        yy = -point.x;
        zz = point.y;
        point.x = xx;
        point.y = yy;
        point.z = zz; //sumar altura camara

        point.r = pr;
        point.g = pg;
        point.b = pb;

        point.u = u;
        point.v = v;

        *pt = point;

    }



    return ret;
}

int Stereo::closeStereoFile() {
    if (m_leftFile == NULL)
        fclose(m_leftFile);
    if (m_rightFile == NULL)
        fclose(m_rightFile);
    m_images_stereo.left_image.~Mat();
    m_images_stereo.right_image.~Mat();
    return 0;
}

Mat Stereo::getLeft() {
    return m_images_stereo.left_image;
}

Mat Stereo::getRight() {
    return m_images_stereo.right_image;
}

Mat Stereo::getUndistLeft() {
    return m_images_stereo.undist_left_image;
}

Mat Stereo::getUndistRight() {
    return m_images_stereo.undist_right_image;
}

Mat Stereo::getDisparity() {
    return m_images_stereo.disp_image;
}

Mat Stereo::getImg3D() {
    return m_images_stereo.recons3D;
}

void Stereo::setLeft(Mat m) {
    m_images_stereo.left_image = m;
}

void Stereo::setRight(Mat m) {
    m_images_stereo.right_image = m;
}

void Stereo::setUndistLeft(Mat m) {
    m_images_stereo.undist_left_image = m;
}

void Stereo::setUndistRight(Mat m) {
    m_images_stereo.undist_right_image = m;
}

void Stereo::setDisparity(Mat m) {
    m_images_stereo.disp_image = m;
}

void Stereo::setImg3D(Mat m) {
    m_images_stereo.recons3D = m;
}

//this function only reads calibration information for camera00 and camera 01 (B/W)

int Stereo::readKittiStereoCalibFiles(string fileName, t_CalibParams * calib_params) {
    int ret = 0;
    char data[500] = {0};
    float dataFl[20] = {0};
    int idCam = 0;
    char matName[100];
    FILE *fd = fopen(fileName.data(), "r");
    string mats[] = {"S_", "K_", "D_", "R_", "T_", "S_rect_", "R_rect_", "P_rect_"};
    int matId = 0;

    if (fd != NULL) {
        while (fscanf(fd, "%s", data) > 0) {
            sprintf(matName, "%s%02d:", mats[matId].data(), idCam);
            //            printf("***%s***\n", data);
            if (strcmp(matName, data) == 0) {
                switch (matId) {
                    case 0://S
                        fscanf(fd, "%f %f", &dataFl[0], &dataFl[1]);
                        this->m_frameWidth = (int) dataFl[0];
                        this->m_frameHeight = (int) dataFl[1];
                        break;
                    case 1://K
                        fscanf(fd, "%f %f %f %f %f %f %f %f %f", &dataFl[0], &dataFl[1],
                               &dataFl[2], &dataFl[3], &dataFl[4], &dataFl[5], &dataFl[6],
                               &dataFl[7], &dataFl[8]);
                        for (int i = 0; i < 9; i++) {
                            calib_params->camera[idCam].matrix[i] = dataFl[i];
                        }
                        break;
                    case 2://D
                        fscanf(fd, "%f %f %f %f %f", &dataFl[0], &dataFl[1],
                               &dataFl[2], &dataFl[3], &dataFl[4]);
                        for (int i = 0; i < 5; i++) {
                            calib_params->camera[idCam].distortion[i] = dataFl[i];
                        }
                        break;
                    case 3://R
                        fscanf(fd, "%f %f %f %f %f %f %f %f %f", &dataFl[0], &dataFl[1],
                               &dataFl[2], &dataFl[3], &dataFl[4], &dataFl[5], &dataFl[6],
                               &dataFl[7], &dataFl[8]);
                        for (int i = 0; i < 9; i++) {
                            calib_params->rot_matrix[i] = dataFl[i];
                        }
                        break;
                    case 4://T
                        fscanf(fd, "%f %f %f", &dataFl[0], &dataFl[1], &dataFl[2]);
                        for (int i = 0; i < 3; i++) {
                            calib_params->trans_vector[i] = dataFl[i];
                        }
                        matId = -1;
                        //solamente uso cam0 y cam1
                        if (idCam == 0)
                            idCam++;
                        else
                            idCam = -1;
                        break;
                }
                matId++;
            }
        }
        printf("Calibration File Loaded.\n");
    }
    else {
        printf("Calibration File Not Found. %s\n", fileName.data());
        ret = -1;
    }

    return ret;
}

int Stereo::readNachoStereoFile(string pathFile, t_CalibParams * calib_params) {

    //% Escribe en fichero: "calib_estereo_param.dat"     % 
    //%    - fxl, fyl, u0l, v0l (floats)                  %
    //%    - fxr, fyr, u0r, v0r (floats)                  %
    //%    - matriz fundamental [9] (floats)              %
    //%    - coeficientes distorsion left [4] (floats)    %
    //%    - coeficientes distorsion right [4] (floats)   %
    //%    - matriz rotaci�n est�reo [9] (floats)         %
    //%    - matriz traslaci�n est�reo [3] (floats)       %
    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    double data[37];
    FILE *fd = fopen(pathFile.data(), "r");
    if (fd == NULL) {
        printf("Error reading stereo calibration file: %s\n", pathFile.data());
        return -1;
    }
    else {
        int cnt = fread(data, sizeof (double), 37, fd);

    }
    //    for(int i=0; i<37; i++){
    //        printf("%d = %.8f\n",i,data[i]);
    //    }

    calib_params->camera[0].matrix[0] = data[0];
    calib_params->camera[0].matrix[1] = 0;
    calib_params->camera[0].matrix[2] = data[2];
    calib_params->camera[0].matrix[3] = 0;
    calib_params->camera[0].matrix[4] = data[1];
    calib_params->camera[0].matrix[5] = data[3];
    calib_params->camera[0].matrix[6] = 0;
    calib_params->camera[0].matrix[7] = 0;
    calib_params->camera[0].matrix[8] = 1;

    calib_params->camera[1].matrix[0] = data[4];
    calib_params->camera[1].matrix[1] = 0;
    calib_params->camera[1].matrix[2] = data[6];
    calib_params->camera[1].matrix[3] = 0;
    calib_params->camera[1].matrix[4] = data[5];
    calib_params->camera[1].matrix[5] = data[7];
    calib_params->camera[1].matrix[6] = 0;
    calib_params->camera[1].matrix[7] = 0;
    calib_params->camera[1].matrix[8] = 1;

    calib_params->fund_matrix[0] = data[8];
    calib_params->fund_matrix[1] = data[9];
    calib_params->fund_matrix[2] = data[10];
    calib_params->fund_matrix[3] = data[11];
    calib_params->fund_matrix[4] = data[12];
    calib_params->fund_matrix[5] = data[13];
    calib_params->fund_matrix[6] = data[14];
    calib_params->fund_matrix[7] = data[15];
    calib_params->fund_matrix[8] = data[16];

    calib_params->camera[0].distortion[0] = data[17];
    calib_params->camera[0].distortion[1] = data[18];
    calib_params->camera[0].distortion[2] = data[19];
    calib_params->camera[0].distortion[3] = data[20];

    calib_params->camera[1].distortion[0] = data[21];
    calib_params->camera[1].distortion[1] = data[22];
    calib_params->camera[1].distortion[2] = data[23];
    calib_params->camera[1].distortion[3] = data[24];

    calib_params->rot_matrix[0] = data[25];
    calib_params->rot_matrix[1] = data[26];
    calib_params->rot_matrix[2] = data[27];
    calib_params->rot_matrix[3] = data[28];
    calib_params->rot_matrix[4] = data[29];
    calib_params->rot_matrix[5] = data[30];
    calib_params->rot_matrix[6] = data[31];
    calib_params->rot_matrix[7] = data[32];
    calib_params->rot_matrix[8] = data[33];

    calib_params->trans_vector[0] = data[34];
    calib_params->trans_vector[1] = data[35];
    calib_params->trans_vector[2] = data[36];

    calib_params->camera[0].img_size[0] = 640;
    calib_params->camera[0].img_size[1] = 480;

    calib_params->camera[1].img_size[0] = 640;
    calib_params->camera[1].img_size[1] = 480;

    printf("Stereo Calibration File Read. OK\n");

    return 0;
}

/** 
 * Read the file with the calibration parameters and save the information in the appropiate structure.
 * @param IN file_name: Name of file where the the calibration parameters are saved.
 * @param OUT calib_params: Parameters to calculate stereo. 
 *
 * @return int value is returned. -1 if any parameters have not been readed, 0 if all parameters have been readed correctly.
 **/
int Stereo::LoadCalibParams(const char* file_name, t_CalibParams * calib_params) {
    // Variables.
    int i = 0; // Index for loop.
    int j = 0; // Index for loop.
    int ret_fs = 0; // Readed bytes.
    FILE* f = NULL; // Pointer to file.
    // End variables.

    // Open the file with the information about the stereo and correlation parameters.
    f = fopen((const char *) file_name, "r");
    if (!f) {
        printf("File hasn't been opened.\n");
        return -1;
    }
    else {
        // Load camera parameters.
        for (i = 0; i < 2; i++) {
            if (i == 0) {
                ret_fs = fscanf(f, "LeftCameraParams:");
            }
            else {
                ret_fs = fscanf(f, "RightCameraParams:");
            }

            ret_fs = fscanf(f, "\n");

            // Read image size.
            for (j = 0; j < 2; j++) {
                ret_fs = fscanf(f, "%d ", &(calib_params->camera[i].img_size[j]));
                if (ret_fs != 1) {
                    printf("\nERROR in LoadCalibParams: size of image hasn't been readed\n");
                    return -1;
                }
            }

            ret_fs = fscanf(f, "\n");

            // Read the matrix of intrinsic parameters.
            for (j = 0; j < 9; j++) {
                ret_fs = fscanf(f, "%lf ", &(calib_params->camera[i].matrix[j]));
                if (ret_fs != 1) {
                    printf("\nERROR in LoadCoalibParams: the matrix of intrinsic parameters hasn't been readed\n");
                    return -1;
                }
            }

            ret_fs = fscanf(f, "\n");

            // Read distorsion parameters.
            for (j = 0; j < 4; j++) {
                ret_fs = fscanf(f, "%lf ", &(calib_params->camera[i].distortion[j]));
                if (ret_fs != 1) {
                    printf("\nERROR in LoadCalibParams: distortion parameters haven't been readed\n");
                    return -1;
                }
            }

            ret_fs = fscanf(f, "\n");
        }


        // Load stereo parameters.
        ret_fs = fscanf(f, "StereoParams:");

        ret_fs = fscanf(f, "\n");

        // Read the fundamental matrix.
        for (i = 0; i < 9; i++) {
            ret_fs = fscanf(f, "%lf ", &(calib_params->fund_matrix[i]));
            if (ret_fs != 1) {
                printf("\nERROR in LoadCalibParams: the fundamental matrix hasn't been readed\n");
                return -1;
            }
        }

        ret_fs = fscanf(f, "\n");

        // Read rotation matrix.
        for (i = 0; i < 9; i++) {
            ret_fs = fscanf(f, "%lf ", &(calib_params->rot_matrix[i]));
            if (ret_fs != 1) {
                printf("\nERROR in LoadCalibParams: the rotation matrix hasn't been readed\n");
                return -1;
            }
        }

        ret_fs = fscanf(f, "\n");

        // Read the translation vector.
        for (i = 0; i < 3; i++) {
            ret_fs = fscanf(f, "%lf ", &(calib_params->trans_vector[i]));
            if (ret_fs != 1) {
                printf("\nERROR in LoadCalibParams: the translation vector hasn't been readed\n");
                return -1;
            }
        }

        ret_fs = fscanf(f, "\n");

        // Read the yaw angle.
        ret_fs = fscanf(f, "%lf ", &(calib_params->yaw));
        if (ret_fs != 1) {
            printf("\nERROR in LoadCalibParams: the yaw angle hasn't been readed\n");
            return -1;
        }

        ret_fs = fscanf(f, "\n");
        ret_fs = fscanf(f, "\n");

        // Read the pitch angle.
        ret_fs = fscanf(f, "%lf ", &(calib_params->pitch));
        if (ret_fs != 1) {
            printf("\nERROR in LoadCalibParams: the pitch angle hasn't been readed\n");
            return -1;
        }

        ret_fs = fscanf(f, "\n");
        ret_fs = fscanf(f, "\n");

        // Read roll angle.
        ret_fs = fscanf(f, "%lf ", &(calib_params->roll));
        if (ret_fs != 1) {
            printf("\nERROR in LoadCalibParams: the roll angle hasn't been readed\n");
            return -1;
        }

        ret_fs = fscanf(f, "\n");

        // Read the height that cameras are positioned.
        ret_fs = fscanf(f, "%lf ", &(calib_params->height));
        if (ret_fs != 1) {
            printf("\nERROR in LoadCalibParams: the height where cameras are situated hasn't been readed\n");
            return -1;
        }

        // Close the file.
        fclose(f);

        return 0;
    }
}

/** 
 * Computes the stereo information.
 * @param IN calib_params: Parameters to calculate stereo. 
 * @param OUT map_matrices: Matrices for computing the distorsion and rectification.
 * @param OUT bm_state: Block matching structure for computing the disparity map.
 *
 * @return void value is returned. 
 **/
void Stereo::ComputeStereo(t_CalibParams *calib_params, t_Map *map_matrices, cv::StereoBM * bm_state) {
    // Variables.
    double M1[3][3]; //Intrinsic matrix of the left camera.
    double M2[3][3]; //Intrinsic matrix of the right camera.
    double D1[5]; //Distorsion of the left camera.
    double D2[5]; //Distorsion of the right camera.
    double R[3][3]; // Rotation matrix between cameras.
    double T[3]; //Translation vector between cameras.
    double F[3][3]; //Fundamental matrix.
    double R1[3][3]; //Rotation's matrix with rectification transform of left camera.
    double R2[3][3]; //Rotation's matrix with rectification transform of right camera.
    double P1[3][4]; //Projection matrix in the new coordinate system of left camera.
    double P2[3][4]; //Projection matrix in the new coordinate system of right camera.

    int i = 0, j = 0; //Indexs to use matrices.
    // End local variables.

    // Load matrices.
    // Fundamental matrix.
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++)
            F[i][j] = calib_params->fund_matrix[3 * i + j];
    }

    // Rotation Matrix.
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++)
            R[i][j] = calib_params->rot_matrix[3 * i + j];
    }

    // Left and right distortion vectors.
    for (i = 0; i < 4; i++) {
        D1[i] = calib_params->camera[0].distortion[i];
    }
    for (i = 0; i < 4; i++) {
        D2[i] = calib_params->camera[1].distortion[i];
    }

    // Translation vector.
    for (i = 0; i < 3; i++) {
        T[i] = calib_params->trans_vector[i];
    }

    // Matrix of intrinsic parameters.
    M1[0][0] = calib_params->camera[0].matrix[0];
    M1[0][1] = calib_params->camera[0].matrix[1];
    M1[0][2] = calib_params->camera[0].matrix[2];
    M1[1][0] = calib_params->camera[0].matrix[3];
    M1[1][1] = calib_params->camera[0].matrix[4];
    M1[1][2] = calib_params->camera[0].matrix[5];
    M1[2][0] = calib_params->camera[0].matrix[6];
    M1[2][1] = calib_params->camera[0].matrix[7];
    M1[2][2] = calib_params->camera[0].matrix[8];
    M2[0][0] = calib_params->camera[1].matrix[0];
    M2[0][1] = calib_params->camera[1].matrix[1];
    M2[0][2] = calib_params->camera[1].matrix[2];
    M2[1][0] = calib_params->camera[1].matrix[3];
    M2[1][1] = calib_params->camera[1].matrix[4];
    M2[1][2] = calib_params->camera[1].matrix[5];
    M2[2][0] = calib_params->camera[1].matrix[6];
    M2[2][1] = calib_params->camera[1].matrix[7];
    M2[2][2] = calib_params->camera[1].matrix[8];

    cv::Mat _M1(3, 3, CV_64FC1, M1);
    cv::Mat _M2(3, 3, CV_64FC1, M2);
    cv::Mat _D1(5, 1, CV_64FC1, D1);
    cv::Mat _D2(5, 1, CV_64FC1, D2);
    cv::Mat _R(3, 3, CV_64FC1, R);
    cv::Mat _T(3, 1, CV_64FC1, T);
    cv::Mat _F(3, 3, CV_64FC1, F);
    cv::Mat _R1(3, 3, CV_64FC1, R1);
    cv::Mat _R2(3, 3, CV_64FC1, R2);
    cv::Mat _P1(3, 4, CV_64FC1, P1);
    cv::Mat _P2(3, 4, CV_64FC1, P2);
    cv::stereoRectify(_M1, _D1, _M2, _D2, cv::Size(calib_params->camera[1].img_size[0], calib_params->camera[1].img_size[1]), _R, _T, _R1, _R2, _P1, _P2, map_matrices->mq, CV_CALIB_ZERO_DISPARITY);
    cv::initUndistortRectifyMap(_M1, _D1, _R1, _P1, cv::Size(calib_params->camera[1].img_size[0], calib_params->camera[1].img_size[1]), CV_32FC1, map_matrices->mrx1, map_matrices->mry1);
    cv::initUndistortRectifyMap(_M2, _D2, _R2, _P2, cv::Size(calib_params->camera[1].img_size[0], calib_params->camera[1].img_size[1]), CV_32FC1, map_matrices->mrx2, map_matrices->mry2);

    // Modify block matching stereo correspondence structure.
    bm_state->state->preFilterType = CV_STEREO_BM_NORMALIZED_RESPONSE;
    bm_state->state->preFilterSize = 7; /*5 to 21*/
    bm_state->state->preFilterCap = 21;
    bm_state->state->SADWindowSize = 19;
    bm_state->state->minDisparity = -2;
    bm_state->state->numberOfDisparities = 16 * 2;
    bm_state->state->textureThreshold = 1;
    bm_state->state->uniquenessRatio = 1;
    bm_state->state->speckleWindowSize = 21; /*9 to 21*/
    bm_state->state->speckleRange = 17;
}

/**
 * Init the process for computing stereovision.
 * @param IN map_matrices: Matrices for computing the distorsion and rectification.
 * @param OUT calib_params: Parameters to calculate stereo. 
 * @param OUT bm_state: Block matching structure for computing the disparity map.
 * 
 * @return int value is returned. -1 if maps are NULL, 0 if maps aren't NULL. 
 **/
int Stereo::InitStereo(t_Map *map_matrices, t_CalibParams *calib_params, cv::StereoBM *bm_state, string calibFile) {
    // Loads calibration parameters of the calibration file.
    //if (LoadCalibParams(CALIB_FILE_NAME, calib_params) == -1)

    if (readNachoStereoFile(calibFile, calib_params) == -1) {
        printf("Calibration parameters have been readed uncorrectly. \n");
        return -1;
    }
    else
        printf("Calibration parameters have been readed correctly. \n");

    // Computes the stereo information.
    ComputeStereo(calib_params, map_matrices, bm_state);

    return 0;
}

//bool ptIsIntoPolygon(vector<> poly)


///**
// * Get the point cloud using disparity map image.
// * @param IN map_matrices: Matrices for computing the distorsion and rectification.
// * @param IN images_stereo: Images. 
// * @param OUT point_cloud: Point cloud from disparity.
// * 
// * @return void value is returned.
// **/
//void Stereo::GetPointCloudStereo(t_Map *map_matrices, t_ImagesStereo *images_stereo, t_PointCloud *point_cloud)
//{
//    // Variables.
//    double px, py, pz, pw;
//    uchar pr, pg, pb;
//    int i, j;
//    t_Point point;
//    uint32_t rgb;
//    uchar d;
//    uchar* rgb_ptr;
//    uchar* disp_ptr;
//    // End variables.
//
//    point_cloud->points.clear();
//    //------------
////    cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(11, 11));
////    cv::erode(images_stereo->vdisp_image, images_stereo->vdisp_image, element);
////    cv::dilate(images_stereo->vdisp_image, images_stereo->vdisp_image, element);
//    //------------
//    
//    for (i = 0; i < images_stereo->vdisp_image.rows - 1; i++)
//    {
//        rgb_ptr = images_stereo->undist_left_image.ptr<uchar > (i);
//        disp_ptr = images_stereo->vdisp_image.ptr<uchar > (i);
//
//        for (j = 0; j < images_stereo->vdisp_image.cols - 1; j++)
//        {
//            if (/*ptIsIntoPolygon(poly, j, i)*/1){
//                // Get 3D coordinates.
//                d = disp_ptr[j];
//
//                // Get RGB information.
//                pb = rgb_ptr[images_stereo->undist_left_image.channels() * j];
//                pg = rgb_ptr[images_stereo->undist_left_image.channels() * j + 1];
//                pr = rgb_ptr[images_stereo->undist_left_image.channels() * j + 2];
//
//                if (d == 0)
//                    continue;
//
//                pw = -1.0 * (static_cast<double> (d)) * map_matrices->mq.at<double>(3, 2) + map_matrices->mq.at<double>(3, 3);
//                px = static_cast<double> (j) + map_matrices->mq.at<double>(0, 3);
//                py = static_cast<double> (i) + map_matrices->mq.at<double>(1, 3);
//                pz = map_matrices->mq.at<double>(2, 3);
//
//                px = px / pw;
//                py = py / pw;
//                pz = pz / pw;
//
//                // Insert information into point cloud structure.      
//                point.x = -px/1000.0;
//                point.y = py/1000.0;
//                point.z = pz/1000.0;
//
//                double xx,yy,zz;
//                xx = -point.z;
//                yy = -point.x;
//                zz = point.y;
//                point.x = xx;
//                point.y = yy;
//                point.z = 1.50+zz;
//
//    //            point.x = -pz/1000.0;
//    //            point.y = px/1000.0;
//    //            point.z = 1.50+pz/1000.0;
//
//                point.r = pr;
//                point.g = pg;
//                point.b = pb;
//
//                point.u = j;
//                point.v = i;
//
//                point_cloud->points.push_back(point);
//            }
//        }
//    }
//    point_cloud->size = (int) point_cloud->points.size();
//}

/**
 * Get the disparity map using stereovision.
 * @param IN map_matrices: Matrices for computing the distorsion and rectification.
 * @param IN bm_state: Block matching structure for computing the disparity map.
 * @param OUT images_stereo: Images. 
 * 
 * @return void value is returned.
 **/
void Stereo::GetDisparityMapStereo(t_Map *map_matrices, cv::StereoBM *bm_state, t_ImagesStereo * images_stereo) {
    // Applies a generic geometrical transformation to the images of video. 
    cv::remap(images_stereo->left_image, images_stereo->undist_left_image, map_matrices->mrx1, map_matrices->mry1, cv::INTER_CUBIC, cv::BORDER_CONSTANT, 0);
    cv::remap(images_stereo->right_image, images_stereo->undist_right_image, map_matrices->mrx2, map_matrices->mry2, cv::INTER_CUBIC, cv::BORDER_CONSTANT, 0);

    //    cv::cvtColor(images_stereo->undist_left_image, images_stereo->undist_left_image_gray, CV_RGB2GRAY);
    //    cv::cvtColor(images_stereo->undist_right_image, images_stereo->undist_right_image_gray, CV_RGB2GRAY);

    // Computes the disparity map using block matching algorithm and normalize. 
    bm_state->operator()(images_stereo->undist_left_image, images_stereo->undist_right_image, images_stereo->disp_image, CV_16S);
    //    cv::filterSpeckles(images_stereo->disp_image, 0, MAX_SPECKLE_SIZE, MAX_DIFF_NEIGHBOR_DISP);

    //    images_stereo->disp_image.convertTo(images_stereo->vdisp_image, CV_8U, 1.0 / 16.0);

    //    openCv example
    double minVal;
    double maxVal;
    minMaxLoc(images_stereo->disp_image, &minVal, &maxVal);
    //images_stereo->disp_image.convertTo( images_stereo->vdisp_image, CV_8UC1, 255/(maxVal - minVal));//EL QUE TENIA YO ANTES
    images_stereo->disp_image.convertTo(images_stereo->vdisp_image, CV_8UC1, 1.0 / 16); //BUENA RAUL

    images_stereo->recons3D = cv::Mat(images_stereo->disp_image.size(), CV_32FC3);
    //    cv::reprojectImageTo3D(images_stereo->disp_image,images_stereo->recons3D,map_matrices->mq,true);//EL QUE TENIA YO ANTES   

    cv::reprojectImageTo3D(images_stereo->vdisp_image, images_stereo->recons3D, map_matrices->mq, false, -1); //BUENA RAUL

}

/** 
 * Allocates memory for images. 
 * @param IN calib_param: Configuration parameters from camera to read the size of images. 
 * @param OUT images_stereo: Images. 
 *
 * @return int value is returned. 0 if all images hasve been allocated correctly else -1 if any image hasn't been allocated. 
 **/
int Stereo::AllocImagesStereo(t_CalibParams* calib_params, t_ImagesStereo * images_stereo) {
    images_stereo->left_image = cv::Mat(cv::Size(calib_params->camera[1].img_size[0], calib_params->camera[1].img_size[1]), CV_8UC3);
    images_stereo->right_image = cv::Mat(cv::Size(calib_params->camera[1].img_size[0], calib_params->camera[1].img_size[1]), CV_8UC3);
    return 0;
}

/**
 * Show images that have been used by stereovision.
 * @param IN images: Images. 
 * 
 * @return void value is returned.
 **/
void Stereo::ShowImagesStereo(t_ImagesStereo * images_stereo) {
    cv::imshow("Undistorted Left Image", images_stereo->undist_left_image);
    cv::imshow("Undistorted Right Image", images_stereo->undist_right_image);
    cv::imshow("Disp", images_stereo->vdisp_image);
}

/**
 * Save images and point cloud that have been used by stereovision.
 * @param IN dir_str: Directory where the images and point cloud will be saved.
 * @param IN frame: Index to create the name of the file that will be saved.
 * @param IN images_stereo: Images.
 * 
 * @return void value is returned.
 **/
void Stereo::SaveImagesStereo(char dir_str[], int frame, t_ImagesStereo * images_stereo) {
    // Variables.
    char name[200] = "";
    // End variables.

    sprintf(name, "%s/undist_left_image_%d.png", dir_str, frame);
    cv::imwrite(name, images_stereo->undist_left_image);

    sprintf(name, "%s/undist_right_image_%d.png", dir_str, frame);
    cv::imwrite(name, images_stereo->undist_right_image);

    sprintf(name, "%s/vdisp_image_%d.png", dir_str, frame);
    cv::imwrite(name, images_stereo->vdisp_image);
}

/**
 * Load images and point cloud that have been used by stereovision and they are saved in dir_str.
 * @param IN dir_str: Directory where the images and point cloud will be saved.
 * @param IN frame: Index to create the name of the file that will be saved.
 * @param OUT images_stereo: Images.
 * 
 * @return int value is returned. = if all images and point cloud have been loaded correctly. -1 if the point cloud haven't been loaded correctly.
 **/
void Stereo::LoadImagesStereo(char dir_str[], int frame, t_ImagesStereo * images_stereo) {
    // Variables.
    char name[200] = "";
    // End variables.

    sprintf(name, "%s/undist_left_image_%d.png", dir_str, frame);
    images_stereo->undist_left_image = cv::imread(name, CV_LOAD_IMAGE_COLOR);

    sprintf(name, "%s/undist_right_image_%d.png", dir_str, frame);
    images_stereo->undist_right_image = cv::imread(name, CV_LOAD_IMAGE_COLOR);

    sprintf(name, "%s/vdisp_image_%d.png", dir_str, frame);
    images_stereo->vdisp_image = cv::imread(name, CV_LOAD_IMAGE_GRAYSCALE);

    cv::cvtColor(images_stereo->undist_left_image, images_stereo->undist_left_image_gray, CV_RGB2GRAY);
    cv::cvtColor(images_stereo->undist_right_image, images_stereo->undist_right_image_gray, CV_RGB2GRAY);
}

int Stereo::exampleOpencv(t_ImagesStereo *images_stereo, t_Map *map_matrices, cv::StereoBM bm_state, Mat imgMask) {
    //    //-- 1. Read the images
    //  Mat imgLeft = imread( argv[1], CV_LOAD_IMAGE_GRAYSCALE );
    //  Mat imgRight = imread( argv[2], CV_LOAD_IMAGE_GRAYSCALE );
    //-- And create the image in which we will save our disparities


    Mat imgDisparity16S = Mat(images_stereo->left_image.rows, images_stereo->left_image.cols, CV_16S);
    Mat imgDisparity8U = Mat(images_stereo->left_image.rows, images_stereo->left_image.cols, CV_8UC1);

    // Applies a generic geometrical transformation to the images of video. 
    cv::remap(images_stereo->left_image, images_stereo->undist_left_image, map_matrices->mrx1, map_matrices->mry1, cv::INTER_CUBIC, cv::BORDER_CONSTANT, 0);
    cv::remap(images_stereo->right_image, images_stereo->undist_right_image, map_matrices->mrx2, map_matrices->mry2, cv::INTER_CUBIC, cv::BORDER_CONSTANT, 0);

    //  //-- 2. Call the constructor for StereoBM
    //  //int ndisparities = 16*5;   /**< Range of disparity */
    //    int ndisparities = 16*1;   /**< Range of disparity */
    //  int SADWindowSize = 21; /**< Size of the block window. Must be odd */
    //
    //  StereoBM sbm( StereoBM::BASIC_PRESET,
    //								ndisparities, 
    //                SADWindowSize );

    //-- 3. Calculate the disparity image
    bm_state(images_stereo->undist_left_image, images_stereo->undist_right_image, imgDisparity16S, CV_16S);

    //-- Check its extreme values
    double minVal;
    double maxVal;

    minMaxLoc(imgDisparity16S, &minVal, &maxVal);

    //  printf("Min disp: %f Max value: %f \n", minVal, maxVal);

    //-- 4. Display it as a CV_8UC1 image
    imgDisparity16S.convertTo(imgDisparity8U, CV_8UC1, 255 / (maxVal - minVal));
    bitwise_and(imgDisparity8U, imgMask, imgDisparity8U);
    images_stereo->vdisp_image = imgDisparity8U;
    cv::Mat erodeWindow = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
    cv::erode(images_stereo->vdisp_image, images_stereo->vdisp_image, erodeWindow);

    cv::Mat dilateWindow = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(11, 11));
    cv::dilate(images_stereo->vdisp_image, images_stereo->vdisp_image, dilateWindow);

    return 0;
}

int Stereo::exampleOpencvBase(string imgL, string imgR, cv::Mat *imgDisp, int nDisp, int sadWind) {

    //-- 1. Read the images
    Mat imgLeft = imread(imgL, CV_LOAD_IMAGE_GRAYSCALE);
    Mat imgRight = imread(imgR, CV_LOAD_IMAGE_GRAYSCALE);
    //-- And create the image in which we will save our disparities
    Mat imgDisparity16S = Mat(imgLeft.rows, imgLeft.cols, CV_16S);
    Mat imgDisparity8U = Mat(imgLeft.rows, imgLeft.cols, CV_8UC1);

    printf("(%d)(%d)\n", imgLeft.cols, imgLeft.rows);

    //-- 2. Call the constructor for StereoBM
    int ndisparities = 16 * nDisp; /**< Range of disparity */
    int SADWindowSize = sadWind; /**< Size of the block window. Must be odd */

    StereoBM sbm(StereoBM::BASIC_PRESET, ndisparities, SADWindowSize);

    //-- 3. Calculate the disparity image
    sbm(imgLeft, imgRight, imgDisparity16S, CV_16S);

    //-- Check its extreme values
    double minVal;
    double maxVal;

    minMaxLoc(imgDisparity16S, &minVal, &maxVal);

    printf("Min disp: %f Max value: %f \n", minVal, maxVal);

    //-- 4. Display it as a CV_8UC1 image
    imgDisparity16S.convertTo(imgDisparity8U, CV_8UC1, 255 / (maxVal - minVal));
    *(imgDisp) = imgDisparity8U;


    //                    namedWindow("windowDisparity", CV_WINDOW_NORMAL);
    //    imshow("windowDisparity", imgDisparity8U);
    //    imshow("left", imgLeft);

    //  printf("windowDisparity = (%d x %d)\n",imgDisparity8U.cols,imgDisparity8U.rows);

    //  //-- 5. Save the image
    //  imwrite("SBM_sample.png", imgDisparity16S);

    //  waitKey(0);

    return 0;
}

int Stereo::compute3dKitti(string pcdName, string img25Name, bool save, double pitch, double roll, double yaw, double transX, double transY, double transZ, bool manual) {

    cv::reprojectImageTo3D(m_images_stereo.vdisp_image, m_images_stereo.recons3D, this->m_q, true, -1); //BUENA RAUL
    cv::Mat img25(m_images_stereo.recons3D.rows, m_images_stereo.recons3D.cols, CV_32FC1);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudAux = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

    std::vector<cv::Mat> coordinates(3);
    cv::Mat coord_x, coord_y, coord_z;

    cv::split(m_images_stereo.recons3D, coordinates);
    coord_x = coordinates[0];
    coord_y = coordinates[1];
    coord_z = coordinates[2];

    //create new point cloud   
    cloudAux->points.clear();
    cloudAux->height = m_images_stereo.recons3D.rows;
    cloudAux->width = m_images_stereo.recons3D.cols;

    //add new points to cloud
    for (int i = 0; i < m_images_stereo.recons3D.rows; i++) {
        for (int j = 0; j < m_images_stereo.recons3D.cols; j++) {
            float x = coord_x.at<float>(i, j);
            float y = coord_y.at<float>(i, j);
            float z = coord_z.at<float>(i, j);

            pcl::PointXYZRGB pt;
            pt.x = z;
            pt.y = -x;
            pt.z = -y + 0.0; //1.65 camera height     

            pcl::PointXYZRGB pt2;
            pt2.x = z;
            pt2.y = -x;
            pt2.z = -y;

            pcl::PointXYZRGB pt1;
            pt1.x = 0;
            pt1.y = 0;
            pt1.z = 0;


            //image 2.5D
            float diff_x = pt2.x - pt1.x;
            float diff_y = pt2.y - pt1.y;
            float diff_z = pt2.z - pt1.z;
            float sq = (diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
            float dist = 0;
            if (sq != 0)
                dist = sqrt(sq);
            img25.at<float>(i, j, 0) = dist;


            //            printf("grey(%04d,%04d)%d\n",j,i,m_images_stereo.undist_left_image.at<uchar>(i, j));
            pt.r = m_images_stereo.undist_left_image.at<uchar>(i, j);
            pt.g = m_images_stereo.undist_left_image.at<uchar>(i, j);
            pt.b = m_images_stereo.undist_left_image.at<uchar>(i, j);
            cloudAux->push_back(pt);


            //            x_new = rxx * x + rxy * y + rxz*z;
            //            y_new = ryx * x + ryy * y + ryz*z;
            //            z_new = rzx * x + rzy * y + rzz*z;
            //            coord_x.at<float>(i, j) = x_new;
            //            coord_y.at<float>(i, j) = y_new;
            //            coord_z.at<float>(i, j) = z_new;
        }
    }
    printf("Stereo Point Cloud Created (%d) points\n", cloudAux->points.size());
    m_cloud.setCloud(cloudAux, m_cloud.original);
    if (!manual) {

        pcl::PointIndices ind;
        m_cloud.conditionalRemoval(0, 20, -4, 4, -5, 5, m_cloud.original, ind, m_cloud.cond_removal);
        //calculate pitch, roll to add them to camera settings
        double distTh = 0.03;
        pcl::SACSegmentation<pcl::PointXYZRGB> sacSeg;
        sacSeg.setModelType(pcl::SACMODEL_PLANE);
        sacSeg.setMethodType(pcl::SAC_RANSAC);
        sacSeg.setDistanceThreshold(distTh);
        sacSeg.setOptimizeCoefficients(true);
        m_cloud.getCloud(cloudAux, m_cloud.cond_removal);
        sacSeg.setInputCloud(cloudAux);
        pcl::PointIndices inliers;
        pcl::ModelCoefficients coeff;
        sacSeg.segment(inliers, coeff);

        double radX, radY, radZ;
        this->getPlaneRadiansInclination(coeff, &radX, &radY, &radZ);
        double degX, degY, degZ;
        this->getPlaneDegreeInclination(coeff, &degX, &degY, &degZ);
        printf("inclinationDeg Init X(%.2f) Y(%.2f) Z(%.2f)\n", degX, degY, degZ);
        //printf("inclinationRad X(%.2f) Y(%.2f) Z(%.2f)\n", radX, radY, radZ);
        this->m_calib_params.pitch = -radX;
        this->m_calib_params.roll = -radY;
        this->m_calib_params.yaw = 0;
        this->m_calib_params.height = 1.650;
        //        m_cloud.rotTransCloud(m_cloud.original, m_cloud.convolution, m_calib_params.pitch, m_calib_params.roll, m_calib_params.yaw, 0.0, 0.0, m_calib_params.height);
    }
    else {
        this->m_calib_params.pitch = pcl::deg2rad(pitch);
        this->m_calib_params.roll = pcl::deg2rad(roll);
        this->m_calib_params.yaw = pcl::deg2rad(yaw);
        this->m_calib_params.height = transZ;
        //        m_cloud.rotTransCloud(m_cloud.original, m_cloud.convolution, m_calib_params.pitch, m_calib_params.roll, m_calib_params.yaw, transX, transY, transZ);
    }
    InitializeCamera(m_calib_params.camera[0].matrix[0], m_calib_params.camera[0].matrix[4],
                     m_calib_params.camera[0].matrix[2], m_calib_params.camera[0].matrix[5],
                     m_calib_params.camera[0].distortion[0], m_calib_params.camera[0].distortion[1],
                     m_calib_params.camera[0].distortion[2], m_calib_params.camera[0].distortion[3],
                     m_calib_params.pitch, m_calib_params.yaw, m_calib_params.roll,
                     0.0, m_calib_params.height, 0.0, &m_camLlorca);




    if (save) {
        FileStorage fs(img25Name, FileStorage::WRITE);
        fs << "img25" << img25;
        fs.release();

        m_cloud.writeCloudToFile(pcdName, m_cloud.original);
        //        m_cloud.writeCloudToFile(pcdName, m_cloud.convolution);
    }
}

int Stereo::initLlorcaCam(double pitch, double roll, double yaw, double camHeight) {
    InitializeCamera(m_calib_params.camera[0].matrix[0], m_calib_params.camera[0].matrix[4],
                     m_calib_params.camera[0].matrix[2], m_calib_params.camera[0].matrix[5],
                     m_calib_params.camera[0].distortion[0], m_calib_params.camera[0].distortion[1],
                     m_calib_params.camera[0].distortion[2], m_calib_params.camera[0].distortion[3],
                     pitch, yaw, roll,
                     0.0, camHeight, 0.0, &m_camLlorca);
    return 0;
}

int Stereo::exampleOpencvBaseKitti(string imgL, string imgR, cv::Mat *imgDisp,
                                   int nDisp, int sadWind, int preFiltSize, int preFiltCap, int minDisp, int textureTh,
                                   int uinqRatio, int speckleWind, int speckleRange, int disp12MaxDiff) {

    try {
        Mat imgLeft = imread(imgL, CV_LOAD_IMAGE_GRAYSCALE);
        Mat imgRight = imread(imgR, CV_LOAD_IMAGE_GRAYSCALE);
        Mat imgDisparity32F = Mat(imgLeft.rows, imgLeft.cols, CV_32FC1);

        printf("(%d)(%d)\n", imgLeft.cols, imgLeft.rows);

        StereoBM sbm(StereoBM::BASIC_PRESET, 16 * nDisp, sadWind);
        sbm.state->numberOfDisparities = 16 * nDisp;
        sbm.state->SADWindowSize = sadWind;
        sbm.state->preFilterSize = preFiltSize;
        sbm.state->preFilterCap = preFiltCap;
        sbm.state->minDisparity = minDisp;
        sbm.state->textureThreshold = textureTh;
        sbm.state->uniquenessRatio = uinqRatio;
        sbm.state->speckleWindowSize = speckleWind;
        sbm.state->speckleRange = speckleRange;
        sbm.state->disp12MaxDiff = disp12MaxDiff;

        //-- 3. Calculate the disparity image
        sbm(imgLeft, imgRight, imgDisparity32F, CV_32FC1);

        double minim, maxim;
        Point minPos, maxPos;
        minMaxLoc(imgDisparity32F, &minim, &maxim, &minPos, &maxPos);
        //printf("Min Value Disp = %.3lf\n", minim);
        //printf("Max Value Disp = %.3lf\n", maxim);

        imgDisparity32F.copyTo(m_images_stereo.vdisp_image);
        *(imgDisp) = imgDisparity32F; //m_images_stereo.vdisp_image;

        imgLeft.copyTo(this->m_images_stereo.undist_left_image);
        imgRight.copyTo(this->m_images_stereo.undist_right_image);
    }
    catch (cv::Exception exc){
        printf("Exception in function exampleOpencvBaseKitti\n");
    }

    return 0;
}

int Stereo::sgbmKitti(string imgL, string imgR, cv::Mat *imgDisp,
                      int minDisp, int nDisp, int sadWind, int p1, int p2,
                      int disp12MaxDiff, int preFiltCap,
                      int uinqRatio, int speckleWind, int speckleRange) {

    try {
        Mat imgLeft = imread(imgL, CV_LOAD_IMAGE_GRAYSCALE);
        Mat imgRight = imread(imgR, CV_LOAD_IMAGE_GRAYSCALE);
        Mat imgDisparity32F = Mat(imgLeft.rows, imgLeft.cols, CV_32FC1);
        Mat imgDisparity16S = Mat(imgLeft.rows, imgLeft.cols, CV_16SC1);

        //printf("(%d)(%d)\n", imgLeft.cols, imgLeft.rows);

        StereoSGBM sgbm(minDisp, 16 * nDisp, sadWind, p1, p2, disp12MaxDiff, preFiltCap, uinqRatio, speckleWind, speckleRange, false);

        //-- 3. Calculate the disparity image
        sgbm(imgLeft, imgRight, imgDisparity16S);
        imgDisparity16S.convertTo(imgDisparity32F, CV_32FC1, 1.0 / 16.0);

        double minim, maxim;
        Point minPos, maxPos;
        minMaxLoc(imgDisparity32F, &minim, &maxim, &minPos, &maxPos);
        //printf("Min Value Disp = %.3lf\n", minim);
        //printf("Max Value Disp = %.3lf\n", maxim);

        imgDisparity32F.copyTo(m_images_stereo.vdisp_image);
        *(imgDisp) = imgDisparity32F; //m_images_stereo.vdisp_image;

        imgLeft.copyTo(this->m_images_stereo.undist_left_image);
        imgRight.copyTo(this->m_images_stereo.undist_right_image);
    }
    catch (cv::Exception exc){
        printf("Exception in function sgbmKitti\n");
    }
    return 0;
}

int Stereo::saveDemToBinFile(cv::Mat mat, string binFile) {
    int ret = 0;
    FILE *fd = fopen(binFile.data(), "w");
    if (fd != NULL) {
        int rows = mat.rows;
        int cols = mat.cols;
        fwrite(&rows, sizeof (rows), 1, fd);
        fwrite(&cols, sizeof (cols), 1, fd);
        fwrite(mat.data, sizeof (float)*rows * cols * mat.channels(), 1, fd);
        fclose(fd);
    }
    else {
        ret = -1;
        printf("Error writing file: %s\n", binFile.data());
    }
    return ret;
}

int Stereo::readDemFromBinFile(string binFile, cv::Mat* mat) {
    cv::Mat aux;
    int ret = 0;
    FILE *fd = fopen(binFile.data(), "r");
    if (fd != NULL) {
        int rows, cols;
        fread(&rows, sizeof (rows), 1, fd);
        fread(&cols, sizeof (cols), 1, fd);
        aux = cv::Mat::zeros(rows, cols, CV_32FC(6));
        fread(aux.data, sizeof (float)*rows * cols * aux.channels(), 1, fd);
        fclose(fd);
    }
    else {
        ret = -1;
        printf("Error reading file: %s\n", binFile.data());
    }
    *mat = aux;
    return ret;
}

int Stereo::saveMatToXmlFile(cv::Mat mat, string xmlFile) {
    FileStorage f(xmlFile, FileStorage::WRITE);
    f << "myMat" << mat;
    f.release();
    return 0;
}

int Stereo::readMatFromXmlFile(string xmlFile, cv::Mat* mat) {
    cv::Mat aux;
    FileStorage f(xmlFile, FileStorage::READ);
    f["myMat"] >> aux;
    f.release();
    *mat = aux;
    return 0;
}

int Stereo::computeDEM_maxHist(cv::Mat imgIn, cv::Mat img3d, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                               int cellWidth, int cellHeight, double beansSize, double beanStart, cv::Mat * imgOut, cv::Mat *demFull) {

    CpuTime cpu1, cpu2;
    int ret = 0;
    Mat imgOutAux;



    cvtColor(imgIn, imgOutAux, CV_GRAY2RGB);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2 = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    cpu1.start();
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGB>(*(cloud), *cloud2);
    cpu1.stop();
    printf("copycloud = %s\n", cpu1.getText().data());

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudAux = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

    pcl::ModelCoefficients coeffPlane;
    pcl::PointIndices inliers, ind;
    m_cloud.setCloud(cloud, m_cloud.original);
    cpu1.start();
    m_cloud.conditionalRemoval(0, 10, -4, 4, -5, 5, m_cloud.original, ind, m_cloud.cond_removal);
    cpu1.stop();
    printf("Cond removal gral = %s\n", cpu1.getText().data());
    cpu1.start();
    this->m_cloud.detectOnePlane(m_cloud.cond_removal, ind, &inliers, 0.03, &coeffPlane);
    cpu1.stop();
    printf("plano = %s\n", cpu1.getText().data());

    cpu1.start();
    m_cloud.conditionalRemoval(0, 20, -10, 10, -0.5, 0.5, m_cloud.original, ind, m_cloud.cond_removal);
    cpu1.stop();
    printf("Cond removal +-0.5metros = %s\n", cpu1.getText().data());

    //1) Divide imgIn in cells of size cellWidth x cellHeight    
    int rowInitial = 225;
    int hCellCnt = 1 + (imgIn.cols / cellWidth);
    int vCellCnt = 1 + (imgIn.rows - rowInitial) / cellHeight;
    int offsetCellU[] = {0, cellWidth, cellWidth, 0};
    int offsetCellV[] = {0, 0, cellHeight, cellHeight};

    *demFull = cv::Mat::zeros(vCellCnt, hCellCnt, CV_32FC(6));



    std::vector<cv::Point> result;

    for (int r = 0; r < vCellCnt; r++) {
        for (int c = 0; c < hCellCnt; c++) {
            cpu2.start();
            printf("Computing Cell %d / %d\n", r * hCellCnt + c, hCellCnt * vCellCnt);

            //calculate center
            //            cpu1.start();
            float x, y, z;
            int u = c * cellWidth + cellWidth / 2;
            int v = rowInitial + r * cellHeight + cellHeight / 2;
            double xw, zw;
            Img2WorldY(&m_camLlorca, u, v, 0.0, &xw, &zw);
            fromCam3D_to_cloud((float) xw, 0.0, (float) zw, &x, &y, &z);
            pcl::PointXYZRGB ptc;
            ptc.x = x;
            ptc.y = y;
            ptc.z = z;
            ptc.r = 0;
            ptc.g = 255;
            ptc.b = 0;
            cloud->points.push_back(ptc);
            //            cpu1.stop();
            //            printf("\tCalculo del centro.DEM = %s\n", cpu1.getText().data());

            //u,v,x,y,z
            //calculate hull
            //            cpu1.start();
            cloudAux->points.clear();
            for (int i = 0; i < 4; i++) {
                float ptX, ptY, ptZ;
                int ptU = c * cellWidth + offsetCellU[i];
                int ptV = rowInitial + r * cellHeight + offsetCellV[i];
                Img2WorldY(&m_camLlorca, (double) ptU, (double) ptV, 0.0, &xw, &zw);
                fromCam3D_to_cloud((float) xw, 0.0, (float) zw, &ptX, &ptY, &ptZ);
                pcl::PointXYZRGB pth;
                pth.x = ptX;
                pth.y = ptY;
                pth.z = ptZ;
                pth.r = 0;
                pth.g = 0;
                pth.b = 255;
                cloudAux->points.push_back(pth);

                //cloud->points.push_back(ptc);
            }
            //            cpu1.stop();
            //            printf("\tCalculo del hull.DEM = %s\n", cpu1.getText().data());

            //ptU,ptV,ptX,ptY,ptZ
            //calculate histogram of points in hull and new image points     

            int n = 0, uh, vh;
            float height = 0;
            float xc, yc, zc;
            m_cloud.setCloud(cloudAux, m_cloud.ror);

            cpu1.start();
            m_cloud.modePolygonArea(m_cloud.cond_removal, m_cloud.ror, coeffPlane, beansSize, &height, &n);
            cpu1.stop();
            printf("\tHistograma y Moda.DEM = %s\n", cpu1.getText().data());

            //            cpu1.start();
            fromCloud_to_cam3D(x, y, z + height, &xc, &yc, &zc);
            World2Img(&m_camLlorca, xc, yc, zc, &uh, &vh);
            result.push_back(Point(uh, vh));
            ptc.x = x;
            ptc.y = y;
            ptc.z = z + height;
            ptc.r = 255;
            ptc.g = 0;
            ptc.b = 0;
            cloud->points.push_back(ptc);
            demFull->at<cv::Vec6f>(r, c)[0] = uh;
            demFull->at<cv::Vec6f>(r, c)[1] = vh;
            demFull->at<cv::Vec6f>(r, c)[2] = ptc.x;
            demFull->at<cv::Vec6f>(r, c)[3] = ptc.y;
            demFull->at<cv::Vec6f>(r, c)[4] = ptc.z;
            //            cpu1.stop();
            //            printf("\tReproyeccion a imagen.DEM = %s\n", cpu1.getText().data());

            //            cpu2.stop();
            //            printf("\tTotal.DEM = %s\n", cpu2.getText().data());
        }
    }


    return ret;
}

//devuelve la posicion en la que esta el elemento a buscar. -1 si no lo encuentra

int binarySearch(cv::Mat elements, float elem, int r, int c) {

    int middle, inf = 0, ret = -1;
    int sup = elements.cols - 1;
    if (r == -1)
        sup = elements.rows - 1;


    while (sup - inf > 1) {
        middle = (sup + inf) / 2;
        float elemMiddle;
        if (r == -1)
            elemMiddle = elements.at<float>(middle, c);
        else
            elemMiddle = elements.at<float>(r, middle);

        if (elemMiddle == elem)
            return middle;
        else if (elem > elemMiddle) {
            sup = middle;
        }
        else {
            inf = middle;
        }
    }

    float dist1 = fabs(elem - elements.at<float>(inf, c));
    float dist2 = fabs(elem - elements.at<float>(sup, c));

    if (dist1 <= dist2)
        ret = inf;
    else
        ret = sup;

    return ret;
}

/*{Post: Si se encuentra devuelve true, sino false}*/

int Stereo::computeDEM_maxHistEfficientBinary(cv::Mat imgIn, cv::Mat img3d, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                                              int cellWidth, int cellHeight, double beansSize, double beanStart, cv::Mat * imgOut, cv::Mat *demFull) {

    CpuTime cpu1, cpu2;
    cpu2.start();
    int ret = 0;

    //1) Divide imgIn in cells of size cellWidth x cellHeight-------------------    
    int rowInitial = 225;
    int hCellCnt = 1 + (imgIn.cols / cellWidth);
    int vCellCnt = 1 + (imgIn.rows - rowInitial) / cellHeight;
    printf("dem(R%d C%d)\n", vCellCnt, hCellCnt);
    *demFull = cv::Mat::zeros(vCellCnt, hCellCnt, CV_32FC(6));
    cv::Mat centersX = cv::Mat::zeros(vCellCnt, hCellCnt, CV_32FC(1));
    cv::Mat centersY = cv::Mat::zeros(vCellCnt, hCellCnt, CV_32FC(1));
    float xmax = -9999, ymin = 9999, ymax = -9999;
    vector<t_VecInt> ptsIndex;


    //calculate center----------------------------------------------------------
    cpu1.start();
    t_VecInt newCell;
    for (int r = 0; r < vCellCnt; r++) {
        for (int c = 0; c < hCellCnt; c++) {
            float x, y, z;
            int u = c * cellWidth + cellWidth / 2;
            int v = rowInitial + r * cellHeight + cellHeight / 2;
            double xw, zw;
            Img2WorldY(&m_camLlorca, u, v, 0.0, &xw, &zw);
            fromCam3D_to_cloud((float) xw, 0.0, (float) zw, &x, &y, &z);
            pcl::PointXYZRGB ptc;
            ptc.x = x;
            ptc.y = y;
            ptc.z = z;
            ptc.r = 0;
            ptc.g = 0;
            ptc.b = 0;
            centersX.at<float>(r, c) = x;
            centersY.at<float>(r, c) = y;
            ptsIndex.push_back(newCell);

            if (x > xmax)
                xmax = x;
            if (y < ymin)
                ymin = y;
            if (y > ymax)
                ymax = y;
        }
    }
    cpu1.stop();
    printf("Calculate all centers = %s\n", cpu1.getText().data());

    //filter area of interest---------------------------------------------------
    cpu1.start();
    pcl::PointIndices ind;
    m_cloud.setCloud(cloud, m_cloud.original);
    m_cloud.conditionalRemoval(0, xmax, ymin, ymax, -0.5, 0.5, m_cloud.original, ind, m_cloud.cond_removal);
    m_cloud.getCloud(cloud, m_cloud.cond_removal);
    cpu1.stop();
    printf("Cond removal = %s\n", cpu1.getText().data());

    //detect plane--------------------------------------------------------------    
    cpu1.start();
    pcl::ModelCoefficients coeffPlane;
    pcl::PointIndices inliers;
    ind.indices.clear();
    this->m_cloud.detectOnePlane(m_cloud.cond_removal, ind, &inliers, 0.03, &coeffPlane);
    cpu1.stop();
    printf("plane = %s\n", cpu1.getText().data());

    //associate each point to a cell--------------------------------------------
    cpu1.start();
    int cnt = m_cloud.getPointCount(m_cloud.cond_removal);
    printf("PointCount = %d\n", cnt);
    for (int p = 0; p < cnt; p++) {

        pcl::PointXYZRGB pt = m_cloud.getPoint(m_cloud.cond_removal, p);

        int foundX = binarySearch(centersX, pt.x, -1, 0);
        int foundY = binarySearch(centersY, pt.y, foundX, -1);


        //        printf("P(%d) assigned to R(%d) (C%d)\n", p, foundX, foundY);
        int closer = foundX * hCellCnt + foundY;
        ptsIndex.at(closer).vect.push_back(p);
    }
    cpu1.stop();
    printf("associate each point to a cell = %s\n", cpu1.getText().data());
    //--------------------------------------------------------------------------
    //            int sum = 0;
    //            for (int i=0; i<ptsIndex.size(); i++){
    //                sum += ptsIndex.at(i).vect.size();
    //                printf("cell(%d / %d) = %d points (%d / %d)\n",i,ptsIndex.size(),ptsIndex.at(i).vect.size(),sum,m_cloud.getPointCount(m_cloud.cond_removal));
    //            }    


    //calculate histogram and mode for each cell--------------------------------
    cpu1.start();
    int idx = 0, n = 0;
    float height = 0;
    for (int r = 0; r < vCellCnt; r++) {
        for (int c = 0; c < hCellCnt; c++, idx++) {
            int uOld = c * cellWidth + cellWidth / 2;
            int vOld = rowInitial + r * cellHeight + cellHeight / 2;
            //            printf("Computing Cell %d / %d\n", r * hCellCnt + c, hCellCnt * vCellCnt);
            //histogram---------------------------------------------------------
            //            cpu1.start();
            m_cloud.modePolygonAreaEfficient(m_cloud.cond_removal, ptsIndex.at(idx).vect, coeffPlane, beansSize, &height, &n);
            //            cpu1.stop();
            //            printf("\tHistogram = %s (height=%.3f)\n", cpu1.getText().data(),height);

            //reproject 3D data to img------------------------------------------
            //            cpu1.start();
            int uh, vh;
            float xc, yc, zc;
            float x, y, z;
            x = centersX.at<float>(r, c);
            y = centersY.at<float>(r, c);
            z = height;
            fromCloud_to_cam3D(x, y, z, &xc, &yc, &zc);
            World2Img(&m_camLlorca, xc, yc, zc, &uh, &vh);
            pcl::PointXYZRGB ptc;
            ptc.x = x;
            ptc.y = y;
            ptc.z = z;
            ptc.r = 255;
            ptc.g = 0;
            ptc.b = 0;
            //            cloud->points.push_back(ptc);

            //            printf("center[R%d , C%d](%d pts)(V%d U%d)(X%.3f Y%.3f Z%.3f)-->(V%d U%d)(X%.3f Y%.3f Z%.3f)\n", r, c, ptsIndex.at(idx).vect.size(),
            //                   vOld, uOld, centers.at(idx).x, centers.at(idx).y, centers.at(idx).z,
            //                   vh, uh, x, y, z);


            demFull->at<cv::Vec6f>(r, c)[0] = uh;
            demFull->at<cv::Vec6f>(r, c)[1] = vh;
            demFull->at<cv::Vec6f>(r, c)[2] = ptc.x;
            demFull->at<cv::Vec6f>(r, c)[3] = ptc.y;
            demFull->at<cv::Vec6f>(r, c)[4] = ptc.z;
            //            cpu1.stop();
            //            printf("\treproject 3D data to img = %s\n", cpu1.getText().data());
        }
    }
    cpu1.stop();
    printf("\tHistogram and Mode = %s\n", cpu1.getText().data());

    cpu2.stop();
    printf("\tTOTAL = %s\n", cpu2.getText().data());
    return ret;
}

int Stereo::computeDEM_maxHistEfficient(cv::Mat imgIn, cv::Mat img3d, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                                        int cellWidth, int cellHeight, double beansSize, double beanStart, cv::Mat * imgOut, cv::Mat *demFull) {

    CpuTime cpu1, cpu2;
    cpu2.start();
    int ret = 0;

    //1) Divide imgIn in cells of size cellWidth x cellHeight-------------------    
    int rowInitial = 225;
    int hCellCnt = 1 + (imgIn.cols / cellWidth);
    int vCellCnt = 1 + (imgIn.rows - rowInitial) / cellHeight;
    *demFull = cv::Mat::zeros(vCellCnt, hCellCnt, CV_32FC(6));
    vector<pcl::PointXYZRGB> centers;
    float xmax = -9999, ymin = 9999, ymax = -9999;
    vector<t_VecInt> ptsIndex;


    //calculate center----------------------------------------------------------
    cpu1.start();
    t_VecInt newCell;
    for (int r = 0; r < vCellCnt; r++) {
        for (int c = 0; c < hCellCnt; c++) {
            float x, y, z;
            int u = c * cellWidth + cellWidth / 2;
            int v = rowInitial + r * cellHeight + cellHeight / 2;
            double xw, zw;
            Img2WorldY(&m_camLlorca, u, v, 0.0, &xw, &zw);
            fromCam3D_to_cloud((float) xw, 0.0, (float) zw, &x, &y, &z);
            pcl::PointXYZRGB ptc;
            ptc.x = x;
            ptc.y = y;
            ptc.z = z;
            ptc.r = 0;
            ptc.g = 0;
            ptc.b = 0;
            centers.push_back(ptc);
            ptsIndex.push_back(newCell);

            if (x > xmax)
                xmax = x;
            if (y < ymin)
                ymin = y;
            if (y > ymax)
                ymax = y;
        }
    }
    cpu1.stop();
    printf("Calculate all centers = %s\n", cpu1.getText().data());

    //filter area of interest---------------------------------------------------
    cpu1.start();
    pcl::PointIndices ind, inliers;
    m_cloud.setCloud(cloud, m_cloud.original);
    m_cloud.conditionalRemoval(0, xmax, ymin, ymax, -0.5, 0.5, m_cloud.original, ind, m_cloud.cond_removal);
    m_cloud.getCloud(cloud, m_cloud.cond_removal);
    cpu1.stop();
    printf("Cond removal = %s\n", cpu1.getText().data());

    //associate each point to a cell--------------------------------------------
    cpu1.start();
    int closer = -1;
    float minDist = 9999;
    for (int p = 0; p < m_cloud.getPointCount(m_cloud.cond_removal); p++) {
        minDist = 9999;
        closer = -1;
        for (int i = 0; i < centers.size(); i++) {
            float dist = pcl::squaredEuclideanDistance(centers.at(i), m_cloud.getPoint(m_cloud.cond_removal, p));
            if (dist < minDist) {
                closer = i;
                minDist = dist;
            }
        }
        ptsIndex.at(closer).vect.push_back(p);
    }
    cpu1.stop();
    printf("associate each point to a cell = %s\n", cpu1.getText().data());
    //--------------------------------------------------------------------------
    //    int sum = 0;
    //    for (int i=0; i<ptsIndex.size(); i++){
    //        sum += ptsIndex.at(i).vect.size();
    //        printf("cell(%d / %d) = %d points (%d / %d)\n",i,ptsIndex.size(),ptsIndex.at(i).vect.size(),sum,m_cloud.getPointCount(m_cloud.cond_removal));
    //    }    
    //detect plane--------------------------------------------------------------    
    cpu1.start();
    pcl::ModelCoefficients coeffPlane;
    ind.indices.clear();
    this->m_cloud.detectOnePlane(m_cloud.cond_removal, ind, &inliers, 0.03, &coeffPlane);
    cpu1.stop();
    printf("plane = %s\n", cpu1.getText().data());


    //calculate histogram and mode for each cell--------------------------------
    cpu1.start();
    int idx = 0, n = 0;
    float height = 0;
    for (int r = 0; r < vCellCnt; r++) {
        for (int c = 0; c < hCellCnt; c++, idx++) {
            int uOld = c * cellWidth + cellWidth / 2;
            int vOld = rowInitial + r * cellHeight + cellHeight / 2;
            //            printf("Computing Cell %d / %d\n", r * hCellCnt + c, hCellCnt * vCellCnt);
            //histogram---------------------------------------------------------
            //            cpu1.start();
            m_cloud.modePolygonAreaEfficient(m_cloud.cond_removal, ptsIndex.at(idx).vect, coeffPlane, beansSize, &height, &n);
            //            cpu1.stop();
            //            printf("\tHistogram = %s (height=%.3f)\n", cpu1.getText().data(),height);

            //reproject 3D data to img------------------------------------------
            //            cpu1.start();
            int uh, vh;
            float xc, yc, zc;
            float x, y, z;
            x = centers.at(idx).x;
            y = centers.at(idx).y;
            z = centers.at(idx).z + height;
            fromCloud_to_cam3D(x, y, z, &xc, &yc, &zc);
            World2Img(&m_camLlorca, xc, yc, zc, &uh, &vh);
            pcl::PointXYZRGB ptc;
            ptc.x = x;
            ptc.y = y;
            ptc.z = z;
            ptc.r = 255;
            ptc.g = 0;
            ptc.b = 0;
            cloud->points.push_back(ptc);

            //            printf("center[R%d , C%d](%d pts)(V%d U%d)(X%.3f Y%.3f Z%.3f)-->(V%d U%d)(X%.3f Y%.3f Z%.3f)\n", r, c, ptsIndex.at(idx).vect.size(),
            //                   vOld, uOld, centers.at(idx).x, centers.at(idx).y, centers.at(idx).z,
            //                   vh, uh, x, y, z);


            demFull->at<cv::Vec6f>(r, c)[0] = uh;
            demFull->at<cv::Vec6f>(r, c)[1] = vh;
            demFull->at<cv::Vec6f>(r, c)[2] = ptc.x;
            demFull->at<cv::Vec6f>(r, c)[3] = ptc.y;
            demFull->at<cv::Vec6f>(r, c)[4] = ptc.z;
            //            cpu1.stop();
            //            printf("\treproject 3D data to img = %s\n", cpu1.getText().data());
        }
    }
    cpu1.stop();
    printf("\thistogram and mode = %s\n", cpu1.getText().data());
    cpu2.stop();
    printf("\tTOTAL = %s\n", cpu2.getText().data());
    return ret;
}

int Stereo::updateDEM(cv::Mat demZ, cv::Mat* demFull) {
    int ret = 0;

    for (int r = 0; r < demZ.rows; r++) {
        for (int c = 0; c < demZ.cols; c++) {
            //get data
            float x = demFull->at<cv::Vec6f>(r, c)[2];
            float y = demFull->at<cv::Vec6f>(r, c)[3];
            float z = demZ.at<float>(r, c);
            //converto to img coordinates
            float xc, yc, zc;
            int u, v;
            fromCloud_to_cam3D(x, y, z, &xc, &yc, &zc);
            World2Img(&m_camLlorca, xc, yc, zc, &u, &v);
            //update values
            demFull->at<cv::Vec6f>(r, c)[0] = u;
            demFull->at<cv::Vec6f>(r, c)[1] = v;
            demFull->at<cv::Vec6f>(r, c)[4] = z;
        }
    }

    return ret;
}

int Stereo::showDEM(cv::Mat demFull, cv::Mat imgIn, string windName) {
    cv::Mat img;
    cvtColor(imgIn, img, CV_GRAY2RGB);

    int ret = 0;
    //draw grid
    try {
        for (int r = 0; r < demFull.rows - 1; r++) {
            for (int c = 0; c < demFull.cols - 1; c++) {
                int vertexR[] = {r, r, r + 1, r + 1, r};
                int vertexC[] = {c, c + 1, c + 1, c, c};

                for (int i = 0; i < 4; i++) {
                    int u1 = demFull.at<cv::Vec6f>(vertexR[i], vertexC[i])[0];
                    int v1 = demFull.at<cv::Vec6f>(vertexR[i], vertexC[i])[1];
                    int u2 = demFull.at<cv::Vec6f>(vertexR[i + 1], vertexC[i + 1])[0];
                    int v2 = demFull.at<cv::Vec6f>(vertexR[i + 1], vertexC[i + 1])[1];
                    line(img, Point(u1, v1), Point(u2, v2), CV_RGB(0, 0, 255), 1, 8);
                }
            }
        }
        imshow(windName, img);
    }
    catch (cv::Exception exc){

        printf("Exception in %s at line %d\n", exc.func.data(), exc.line);
        ret = -1;
    }
    return ret;
}

int getBeanIndex(double *beans, double data, double range) {

    bool found = false;
    int ret = -1;
    int size = sizeof (beans) / sizeof (double);
    for (int i = 0; i < size && !found; i++) {
        found = (data >= beans[i] - range) && (data <= beans[i] + range);
        if (found)
            ret = i;
    }
    return ret;
}

int Stereo::calcHistMaxValue(vector<t_Point> pts, pcl::ModelCoefficients coeff, double* beanMin, double* beanMax, int* beanCnt, const double beanSize, const double beanStart) {
    int ret = 0, maxim = 0; //maxim is the maximum value of the histogram
    double minBean = 0, maxBean = 0; //range of maximum histogram bean
    const int totalBeans = fabs(2 * beanStart) / beanSize; //number of beans of the histogram
    double beans[totalBeans]; // = {0}; //values of the beans
    int counter[totalBeans]; // = {0}; //number of points into the bean
    double beanRange = beanSize / 2; //a bean is centered in beans[i]+-beanRange
    //fill beans
    for (int i = 0; i < totalBeans; i++) {
        beans[i] = beanStart + i * beanSize;
        counter[i] = 0;
    }

    for (int i = 0; i < pts.size(); i++) {
        //calculate height
        t_Point pt = pts.at(i);
        pcl::PointXYZRGB ptc;
        ptc.x = pt.x;
        ptc.y = pt.y;
        ptc.z = pt.z;
        double d = distPtPlaneSigned(ptc, coeff);
        //find the bean
        int idx = getBeanIndex(beans, d, beanRange);
        //increment counter
        if (idx != -1) {
            counter[idx] = counter[idx] + 1;
            if (counter[idx] > maxim) {
                maxim = counter[idx];
                minBean = beans[idx] - beanRange;
                maxBean = beans[idx] + beanRange;
            }
        }
    }

    *beanMin = minBean;
    *beanMax = maxBean;
    *beanCnt = maxim;

    return ret;
}

int Stereo::getNearestCell(vector<t_Cell> cells, float x, float y, float z) {
    int nearIdx = -1;
    float nearDst = FLT_MAX;
    //float MAX_DIST_SQ = 3.

    for (int i = 0; i < cells.size(); i++) {
        pcl::PointXYZ pt0(x, y, z);
        pcl::PointXYZ pt1(cells.at(i).center.x, cells.at(i).center.y, cells.at(i).center.z);
        //float dist = pcl::euclideanDistance(pt0, pt1);
        float dist = pcl::squaredEuclideanDistance(pt0, pt1);
        if (dist < nearDst) {
            nearDst = dist;
            nearIdx = i;
        }
    }
    return nearIdx;
}

int Stereo::fromCloud_to_cam3D(float xCloud, float yCloud, float zCloud, float *xCam, float *yCam, float *zCam) {
    *zCam = xCloud;
    *xCam = -yCloud;
    *yCam = -zCloud;
    return 0;
}

int Stereo::fromCam3D_to_cloud(float xCam, float yCam, float zCam, float* xCloud, float* yCloud, float* zCloud) {
    *xCloud = zCam;
    *yCloud = -xCam;
    *zCloud = -yCam;
    return 0;
}

int Stereo::fromCam2D_to_cloud(Mat img3d, int u, int v, float* xCloud, float* yCloud, float* zCloud) {
    Vec3f pt = img3d.at<Vec3f>(v, u);
    this->fromCam3D_to_cloud(pt[0], pt[1], pt[2], xCloud, yCloud, zCloud);
    return 0;
}

double Stereo::distPtPlaneSigned(pcl::PointXYZRGB pt, pcl::ModelCoefficients coeff) {

    double denom = sqrt((coeff.values.at(0) * coeff.values.at(0))+(coeff.values.at(1) * coeff.values.at(1))+(coeff.values.at(2) * coeff.values.at(2)));
    double numAbs = fabs(coeff.values.at(0) * pt.x + coeff.values.at(1) * pt.y + coeff.values.at(2) * pt.z + coeff.values.at(3));
    double num = coeff.values.at(0) * pt.x + coeff.values.at(1) * pt.y + coeff.values.at(2) * pt.z + coeff.values.at(3);
    double dist = num / denom;
    double distAbs = numAbs / denom;
    //    printf("dist = %.3f\ndistAbs = %.3f\n", dist, distAbs);
    //    double zPoint = pt.z;
    //    double zPlane = -(coeff.values.at(0) * pt.x + coeff.values.at(1) * pt.y + coeff.values.at(3))/coeff.values.at(2);
    //    if (zPoint < zPlane)
    //        dist = -dist;

    return dist;
}

void Stereo::getPlaneDegreeInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z) {

    *x = pcl::rad2deg(atan(plane.values.at(0)));
    *y = pcl::rad2deg(atan(plane.values.at(1)));
    *z = pcl::rad2deg(atan(plane.values.at(2)));
}

void Stereo::getPlaneRadiansInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z) {

    *x = atan(plane.values.at(0));
    *y = atan(plane.values.at(1));
    *z = atan(plane.values.at(2));
}