DispModule.cpp

/*
 * Copyright (C) 2015 RobotCub Consortium
 * Author: Sean Ryan Fanello, Giulia Pasquale
 * email:   sean.fanello@iit.it giulia.pasquale@iit.it
 * website: www.robotcub.org
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
 */

#include <cmath>
#include <algorithm>
#include <yarp/cv/Cv.h>
#include "DispModule.h"

// --- DEBUG ---
// this code left on purpose, it was used to carry out profiling
// of the different subroutines in the code
//
// --- SAMPLE USE ---
//
// have a "debug_count" flag and a "debug_timings[]"" array stored stored in your main loop, starting at 0, 
// then for every function to be profiled use:
//
//    PROF_S
//    method_to_be_profiled()
//    PROF_E(<number>)
//
// then you can print every 100 iterations in a way similar to:
//
//    if(++debug_count == 100)
//    {
//        std::cout << std::endl << "---- DISPMODULE: AVERAGE TIMING OVER " << debug_count << " FRAMES ----" << std::endl;
//        for(int i = 0; i < this->debug_num_timings; i++)
//        {
//            std::cout << debug_strings[i] << ": " << (this->debug_timings[i] / debug_count) / 1000. << " ms" << std::endl;
//            this->debug_timings[i] = 0;
//        }
//        debug_count = 0;
//    }
//
// the "debug_strings[]" array stores the strings associated with 
// the different <number> values used in PROF_E(<number>)
//
//#include <chrono>
//#define PROF_S {start = std::chrono::high_resolution_clock::now();}
//#define PROF_E(N) {stop = std::chrono::high_resolution_clock::now();duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start); this->debug_timings[N] += duration.count();}
//
// char const *debug_strings[] = {
//     "updateViaKinematics",
//     "updateViaGazeCtrl",
//     "getCameraHGazkillGUIeCtrlx2",
//     "gui.recalibrate",
//     "runRecalibration",
//     "gui.isUpdated",
//     "computeDisparity",
//     "bilateralFilter",
//     "wlsFilter",
//     "ouputDisparity",
//     "outputDepth"
// };

using namespace yarp::cv;


void DispModule::printParameters()
{
    std::cout << "params ("
              << this->stereo_parameters.minDisparity << " "
              << this->useBestDisp << " "
              << this->stereo_parameters.numberOfDisparities << " "
              << this->stereo_parameters.SADWindowSize << " "
              << this->stereo_parameters.disp12MaxDiff << " "
              << this->stereo_parameters.preFilterCap << " "
              << this->stereo_parameters.uniquenessRatio << " "
              << this->stereo_parameters.speckleWindowSize << " "
              << this->stereo_parameters.speckleRange << " "
              << this->stereo_parameters.sigmaColorBLF << " "
              << this->stereo_parameters.sigmaSpaceBLF << " "
              << this->stereo_parameters.wls_lambda << " "
              << this->stereo_parameters.wls_sigma << " "
              << this->stereo_parameters.stereo_matching << " "
              << this->stereo_parameters.BLFfiltering << " "
              << this->stereo_parameters.WLSfiltering
              << ")" << std::endl;
}



bool DispModule::configure(ResourceFinder & rf)
{
    // populate local variables with infos from the context

    string name=rf.check("name",Value("DisparityModule")).asString();
    string robot=rf.check("robot",Value("icub")).asString();
    string left=rf.check("leftPort",Value("/left:i")).asString();
    string right=rf.check("rightPort",Value("/right:i")).asString();
    string SFMFile=rf.check("SFMFile",Value("SFM.ini")).asString();

    string sname="/"+name;
    left=sname+left;
    right=sname+right;

    // set up output disparity and depth ports

    string outDispName=rf.check("outDispPort",Value("/disp:o")).asString();
    string outDepthName=rf.check("outDepthPort",Value("/depth:o")).asString();

    outDepthName=sname+outDepthName;
    outDispName=sname+outDispName;

    // load the calibration infos from file

    this->localCalibration.setDefaultContext("cameraCalibration");
    this->localCalibration.setDefaultConfigFile(SFMFile.c_str());
    this->localCalibration.configure(0,NULL);

    this->camCalibFile=this->localCalibration.getHomeContextPath();
    this->camCalibFile+="/"+SFMFile;

    // prepares the name for the RPC port

    string rpc_name=sname+"/rpc";

    // open the ports

    leftImgPort.open(left);
    rightImgPort.open(right);
    outDepth.open(outDepthName);
    outDisp.open(outDispName);
    handlerPort.open(rpc_name);
    attach(handlerPort);

    // create the stereocamera and load the
    // intrinsic/extrinsic parameters

    this->stereo = new StereoCamera(true);

    Mat KL, KR, DistL, DistR;

    loadIntrinsics(rf,KL,KR,DistL,DistR);
    loadConfigurationFile(this->localCalibration,R0,T0,eyes0);

    // stores the original parameters 

    this->original_parameters = this->stereo_parameters;

    // auxiliary initialization calls

    eyes.resize(eyes0.length(),0.0);

    stereo->setIntrinsics(KL,KR,DistL,DistR);

    // initialize the matrices describing the
    // transformation between reference systems

    this->HL_root=Mat::zeros(4,4,CV_64F);
    this->HR_root=Mat::zeros(4,4,CV_64F);

    bool useCalibrated = rf.check("useCalibrated");

    // TODO: check whether this part next is needed or not

    if (useCalibrated)
    {
        Mat KL=this->stereo->getKleft();
        Mat KR=this->stereo->getKright();
        Mat zeroDist=Mat::zeros(1,8,CV_64FC1);
        this->stereo->setIntrinsics(KL,KR,zeroDist,zeroDist);
    }

    // auxiliary variables for the calibration process

    this->init=true;
    this->numberOfTrials=0;

#ifdef USING_GPU
    utils=new Utilities();
    utils->initSIFT_GPU();
#endif

    Property optionHead;
    optionHead.put("device","remote_controlboard");
    optionHead.put("remote","/"+robot+"/head");
    optionHead.put("local",sname+"/headClient");

    if (headCtrl.open(optionHead))
    {
        headCtrl.view(iencs);
        iencs->getAxes(&nHeadAxes);
    }
    else
    {
        std::cout << "[DisparityModule] Devices not available" << std::endl;
        return false;
    }

    // set up to interface with the GazeController

    Property optionGaze;
    optionGaze.put("device","gazecontrollerclient");
    optionGaze.put("remote","/iKinGazeCtrl");
    optionGaze.put("local",sname+"/gazeClient");
    if (gazeCtrl.open(optionGaze))
        gazeCtrl.view(igaze);
    else
    {
        std::cout << "[DisparityModule] Devices not available" << std::endl;
        headCtrl.close();
        return false;
    }

    // set up the translation and rotation parameters of the stereo system

    if (!R0.empty() && !T0.empty())
    {
        stereo->setRotation(R0,0);
        stereo->setTranslation(T0,0);
    }
    else
    {
        std::cout << "[DisparityModule] No local calibration file found in " <<  camCalibFile
             << "[DisparityModule] ... Using Kinematics and Running SFM once." << std::endl;
        updateViaGazeCtrl(true);
        R0=this->stereo->getRotation();
        T0=this->stereo->getTranslation();
    }

    this->runRecalibration=false;
    this->updateViaGazeCtrl(false);

    // select the stereo matching algorithm, filtering
    // methods on the basis of the parameters the
    // module has been invoked with, in case the user 
    // prefers to specify it via command line and not 
    // using the GUI

   if (rf.check("sgbm"))
       this->stereo_parameters.stereo_matching = SM_MATCHING_ALG::SGBM_OPENCV;
   else if(rf.check("sgbm_cuda"))
       this->stereo_parameters.stereo_matching = SM_MATCHING_ALG::SGBM_CUDA;
   else if(rf.check("libelas"))
       this->stereo_parameters.stereo_matching = SM_MATCHING_ALG::LIBELAS;

   if (rf.check("blf"))
       this->stereo_parameters.BLFfiltering = SM_BLF_FILTER::BLF_ORIGINAL;
   else if(rf.check("blf_cuda"))
       this->stereo_parameters.BLFfiltering = SM_BLF_FILTER::BLF_CUDA;
   else if(rf.check("no_blf"))
       this->stereo_parameters.BLFfiltering = SM_BLF_FILTER::BLF_DISABLED;

   if (rf.check("wls"))
       this->stereo_parameters.WLSfiltering = SM_WLS_FILTER::WLS_ENABLED;
   else if(rf.check("wls_lr"))
       this->stereo_parameters.WLSfiltering = SM_WLS_FILTER::WLS_LRCHECK;
   else if(rf.check("no_wls"))
       this->stereo_parameters.WLSfiltering = SM_WLS_FILTER::WLS_DISABLED;

    // if specified via CMake, compile using the GUI
    // based on cvui.h and here checks whether to initialize
    // it or not

#ifdef USE_GUI
    this->debugWindow = rf.check("gui");

    if(this->debugWindow)
    {
        this->gui.initializeGUI(this->stereo_parameters.minDisparity,
                                this->stereo_parameters.numberOfDisparities,
                                this->stereo_parameters.SADWindowSize,
                                this->stereo_parameters.disp12MaxDiff,
                                this->stereo_parameters.preFilterCap,
                                this->stereo_parameters.uniquenessRatio,
                                this->stereo_parameters.speckleWindowSize,
                                this->stereo_parameters.speckleRange,
                                this->stereo_parameters.sigmaColorBLF,
                                this->stereo_parameters.sigmaSpaceBLF,
                                this->stereo_parameters.wls_lambda,
                                this->stereo_parameters.wls_sigma,
                                this->stereo_parameters.BLFfiltering,
                                this->stereo_parameters.WLSfiltering,
                                this->stereo_parameters.stereo_matching);
    }
#endif

    // initialize the StereoMatcher object

    this->matcher = new StereoMatcherNew(rf, this->stereo);

    this->matcher->setParameters(this->stereo_parameters.minDisparity,
                                 this->stereo_parameters.numberOfDisparities,
                                 this->stereo_parameters.SADWindowSize,
                                 this->stereo_parameters.disp12MaxDiff,
                                 this->stereo_parameters.preFilterCap,
                                 this->stereo_parameters.uniquenessRatio,
                                 this->stereo_parameters.speckleWindowSize,
                                 this->stereo_parameters.speckleRange,
                                 this->stereo_parameters.sigmaColorBLF,
                                 this->stereo_parameters.sigmaSpaceBLF,
                                 this->stereo_parameters.wls_lambda,
                                 this->stereo_parameters.wls_sigma,
                                 this->stereo_parameters.BLFfiltering,
                                 this->stereo_parameters.WLSfiltering,
                                 this->stereo_parameters.stereo_matching);

    this->matcher->updateCUDAParams();

    return true;
}


cv::Mat DispModule::depthFromDisparity(Mat disparity, Mat Q, Mat R)
{
    // compute the depth from the disparity map

    cv::Mat depth;

    depth = disparity.clone();

    depth.convertTo(depth, CV_32FC1, 1./16.);

    // extract the subelements of the R and Q matrices, 
    // to simplify the further calculations

    float r02 = float(R.at<double>(0,2));
    float r12 = float(R.at<double>(1,2));
    float r22 = float(R.at<double>(2,2));

    float q00 = float(Q.at<double>(0,0));
    float q03 = float(Q.at<double>(0,3));
    float q11 = float(Q.at<double>(1,1));
    float q13 = float(Q.at<double>(1,3));
    float q23 = float(Q.at<double>(2,3));
    float q32 = float(Q.at<double>(3,2));
    float q33 = float(Q.at<double>(3,3));

    const Mat& Mapper=this->stereo->getMapperL();

    if (Mapper.empty())
        return depth;

    float usign, vsign;

    for (int u = 0; u < depth.rows; u++)
        for (int v = 0; v < depth.cols; v++)
        {
            usign=cvRound(Mapper.ptr<float>(u)[2*v]);
            vsign=cvRound(Mapper.ptr<float>(u)[2*v+1]);

            if ((usign<0) || (usign>=depth.cols) || (vsign<0) || (vsign>=depth.rows))
                depth.at<float>(u,v) = 0.0;
            else
                depth.at<float>(u,v) = (r02*(float(vsign)*q00+q03)+r12*(float(usign)*q11+q13)+r22*q23)/(depth.at<float>(vsign,usign)*q32+q33);

        }

    return depth;
}


void DispModule::initializeStereoParams()
{
    this->useBestDisp=true;

    this->stereo_parameters.uniquenessRatio=15;
    this->stereo_parameters.speckleWindowSize=50;
    this->stereo_parameters.speckleRange=16;
    this->stereo_parameters.SADWindowSize=7;
    this->stereo_parameters.minDisparity=0;
    this->stereo_parameters.preFilterCap=63;
    this->stereo_parameters.disp12MaxDiff=0;

    this->stereo_parameters.numberOfDisparities = 96;

    this->stereo_parameters.sigmaColorBLF = 10.0;
    this->stereo_parameters.sigmaSpaceBLF = 10.0;

    this->stereo_parameters.wls_lambda = 4500.;
    this->stereo_parameters.wls_sigma = 1.0;

    this->stereo_parameters.stereo_matching = SM_MATCHING_ALG::SGBM_OPENCV;
    this->stereo_parameters.BLFfiltering = SM_BLF_FILTER::BLF_DISABLED;
    this->stereo_parameters.WLSfiltering = SM_WLS_FILTER::WLS_DISABLED;

    this->original_parameters = this->stereo_parameters;
}


void DispModule::updateViaKinematics(const yarp::sig::Vector& deyes)
{
    double dpan=CTRL_DEG2RAD*deyes[1];
    double dver=CTRL_DEG2RAD*deyes[2];

    yarp::sig::Vector rot_l_pan(4,0.0);
    rot_l_pan[1]=1.0;
    rot_l_pan[3]=dpan+dver/2.0;
    Matrix L1=axis2dcm(rot_l_pan);

    yarp::sig::Vector rot_r_pan(4,0.0);
    rot_r_pan[1]=1.0;
    rot_r_pan[3]=dpan-dver/2.0;
    Matrix R1=axis2dcm(rot_r_pan);

    Mat RT0=buildRotTras(R0,T0);
    Matrix H0; convert(RT0,H0);
    Matrix H=SE3inv(R1)*H0*L1;

    Mat R=Mat::zeros(3,3,CV_64F);
    Mat T=Mat::zeros(3,1,CV_64F);

    for (int i=0; i<R.rows; i++)
        for(int j=0; j<R.cols; j++)
            R.at<double>(i,j)=H(i,j);

    for (int i=0; i<T.rows; i++)
        T.at<double>(i,0)=H(i,3);

    this->stereo->setRotation(R,0);
    this->stereo->setTranslation(T,0);
}



void DispModule::updateViaGazeCtrl(const bool update)
{
    Matrix L1=getCameraHGazeCtrl(LEFT);
    Matrix R1=getCameraHGazeCtrl(RIGHT);

    Matrix RT=SE3inv(R1)*L1;

    Mat R=Mat::zeros(3,3,CV_64F);
    Mat T=Mat::zeros(3,1,CV_64F);

    for (int i=0; i<R.rows; i++)
        for(int j=0; j<R.cols; j++)
            R.at<double>(i,j)=RT(i,j);

    for (int i=0; i<T.rows; i++)
        T.at<double>(i,0)=RT(i,3);

    if (update)
    {
        stereo->setRotation(R,0);
        stereo->setTranslation(T,0);
    }
    else
        stereo->setExpectedPosition(R,T);
}



bool DispModule::interruptModule()
{
    leftImgPort.interrupt();
    rightImgPort.interrupt();
    outDisp.interrupt();
    outDepth.interrupt();
    handlerPort.interrupt();

    return true;
}



bool DispModule::close()
{
    leftImgPort.close();
    rightImgPort.close();
    outDisp.close();
    outDepth.close();
    handlerPort.close();

    headCtrl.close();
    gazeCtrl.close();

    delete stereo;

#ifdef USING_GPU
    delete utils;
#endif


    return true;
}


void DispModule::recalibrate()
{

#ifdef USING_GPU
        utils->extractMatch_GPU(leftMat,rightMat);
        vector<Point2f> leftM,rightM;
        utils->getMatches(leftM,rightM);
        mutexDisp.lock();
        this->stereo->setMatches(leftM,rightM);
#else

        mutexDisp.lock();
        stereo->findMatch(false);
#endif

        stereo->estimateEssential();
        bool ok=stereo->essentialDecomposition();
        mutexDisp.unlock();

        // if the calibration process has been successfull,
        // stores the parameters estimated, otherwise stops the
        // calibration process in case it has failed for five
        // times in a row

        if (ok)
        {
            this->calibUpdated=true;
            this->runRecalibration=false;
            calibEndEvent.signal();

            R0=stereo->getRotation();
            T0=stereo->getTranslation();
            eyes0=eyes;

            std::cout << "[DisparityModule] Calibration Successful!" << std::endl;

        }
        else
        {
            if (++numberOfTrials>5)
            {
                this->calibUpdated=false;
                this->runRecalibration=false;
                calibEndEvent.signal();

                std::cout << "[DisparityModule] Calibration failed after 5 trials.." << std::endl <<
                             "[DisparityModule] ..please show a non planar scene." << std::endl;

            }
        }
}


#ifdef USE_GUI
void DispModule::handleGuiUpdate()
{

    // the calls to gui.set/getParameters and to 
    // matcher->set/getParameters() can be definitely
    // improved, by wrapping everything in a Params struct
    // as defined in StereoMatcher.h, this has to be implemented

    if(this->gui.toRecalibrate())
    {
        std::cout << "[DisparityModule] Updating calibration.." << std::endl;

        mutexRecalibration.lock();
        numberOfTrials=0;
        this->runRecalibration=true;
        mutexRecalibration.unlock();

    }
    else if(this->gui.toSaveCalibration())
    {
        std::cout << "[DisparityModule] Saving current calibration.." << std::endl;
        updateConfigurationFile(R0,T0,eyes0,"STEREO_DISPARITY");
    }
    else if(this->gui.toLoadParameters())
    {
        std::cout << "[DisparityModule] Loading stereo parameters from file.." << std::endl;

        this->stereo_parameters = this->original_parameters;

        this->gui.setParameters(this->stereo_parameters.minDisparity,
                               this->stereo_parameters.numberOfDisparities,
                               this->stereo_parameters.SADWindowSize,
                               this->stereo_parameters.disp12MaxDiff,
                               this->stereo_parameters.preFilterCap,
                               this->stereo_parameters.uniquenessRatio,
                               this->stereo_parameters.speckleWindowSize,
                               this->stereo_parameters.speckleRange,
                               this->stereo_parameters.sigmaColorBLF,
                               this->stereo_parameters.sigmaSpaceBLF,
                               this->stereo_parameters.wls_lambda,
                               this->stereo_parameters.wls_sigma,
                               this->stereo_parameters.BLFfiltering,
                               this->stereo_parameters.WLSfiltering,
                               this->stereo_parameters.stereo_matching);

        this->matcher->setParameters(this->stereo_parameters.minDisparity,
                                     this->stereo_parameters.numberOfDisparities,
                                     this->stereo_parameters.SADWindowSize,
                                     this->stereo_parameters.disp12MaxDiff,
                                     this->stereo_parameters.preFilterCap,
                                     this->stereo_parameters.uniquenessRatio,
                                     this->stereo_parameters.speckleWindowSize,
                                     this->stereo_parameters.speckleRange,
                                     this->stereo_parameters.sigmaColorBLF,
                                     this->stereo_parameters.sigmaSpaceBLF,
                                     this->stereo_parameters.wls_lambda,
                                     this->stereo_parameters.wls_sigma,
                                     this->stereo_parameters.BLFfiltering,
                                     this->stereo_parameters.WLSfiltering,
                                     this->stereo_parameters.stereo_matching);

        this->matcher->updateCUDAParams();
    }
    else if(this->gui.toSaveParameters())
    {
        std::cout << "[DisparityModule] Saving stereo parameters.." << std::endl;
        updateConfigurationFile(R0,T0,eyes0,"STEREO_DISPARITY");
    }
    else
    {

        this->gui.getParameters(this->stereo_parameters.minDisparity,
                                this->stereo_parameters.numberOfDisparities,
                                this->stereo_parameters.SADWindowSize,
                                this->stereo_parameters.disp12MaxDiff,
                                this->stereo_parameters.preFilterCap,
                                this->stereo_parameters.uniquenessRatio,
                                this->stereo_parameters.speckleWindowSize,
                                this->stereo_parameters.speckleRange,
                                this->stereo_parameters.sigmaColorBLF,
                                this->stereo_parameters.sigmaSpaceBLF,
                                this->stereo_parameters.wls_lambda,
                                this->stereo_parameters.wls_sigma,
                                this->stereo_parameters.BLFfiltering,
                                this->stereo_parameters.WLSfiltering,
                                this->stereo_parameters.stereo_matching);

        this->matcher->setParameters(this->stereo_parameters.minDisparity,
                                     this->stereo_parameters.numberOfDisparities,
                                     this->stereo_parameters.SADWindowSize,
                                     this->stereo_parameters.disp12MaxDiff,
                                     this->stereo_parameters.preFilterCap,
                                     this->stereo_parameters.uniquenessRatio,
                                     this->stereo_parameters.speckleWindowSize,
                                     this->stereo_parameters.speckleRange,
                                     this->stereo_parameters.sigmaColorBLF,
                                     this->stereo_parameters.sigmaSpaceBLF,
                                     this->stereo_parameters.wls_lambda,
                                     this->stereo_parameters.wls_sigma,
                                     this->stereo_parameters.BLFfiltering,
                                     this->stereo_parameters.WLSfiltering,
                                     this->stereo_parameters.stereo_matching);

        this->matcher->updateCUDAParams();

    }

    this->gui.resetState();
}
#endif


bool DispModule::updateModule()
{
    // acquire the left and right images

    ImageOf<PixelRgb> *yarp_imgL=leftImgPort.read(true);
    ImageOf<PixelRgb> *yarp_imgR=rightImgPort.read(true);

    Stamp stamp_left, stamp_right;
    leftImgPort.getEnvelope(stamp_left);
    rightImgPort.getEnvelope(stamp_right);

    if ((yarp_imgL==NULL) || (yarp_imgR==NULL))
        return true;

    // read encoders

    iencs->getEncoder(nHeadAxes-3,&eyes[0]);
    iencs->getEncoder(nHeadAxes-2,&eyes[1]);
    iencs->getEncoder(nHeadAxes-1,&eyes[2]);

    // updated the internal state by reading the information
    // from the kinematics of the robot and from the GazeController

    updateViaKinematics(eyes-eyes0);
    updateViaGazeCtrl(false);

    leftMat=toCvMat(*yarp_imgL);
    rightMat=toCvMat(*yarp_imgR);

    // updates left/right eyes pose

    getCameraHGazeCtrl(LEFT);
    getCameraHGazeCtrl(RIGHT);

    // stores the images in the StereoCamera object

    this->stereo->setImages(leftMat,rightMat);

#ifdef USE_GUI
    if(this->debugWindow && this->gui.isUpdated())
        this->handleGuiUpdate();
#endif

    mutexRecalibration.lock();

    if(this->runRecalibration)
        this->recalibrate();

    mutexRecalibration.unlock();

    // compute the current disparity map

    mutexDisp.lock();

    matcher->compute();
    
    mutexDisp.unlock();

    // execute the bilateral filtering of the
    // disparity map, if selected

    matcher->filterBLF("base");

    // execute the WLS filtering, if selected

    matcher->filterWLS("blf");

    // if the disparity output port is active, sends
    // the current (filtered) one

    if (outDisp.getOutputCount() > 0)
    {

        // here, gets the disparity map after the WLS filtering, 
        // which does not imply that it has been actually been 
        // filtered in that way, it's just a convention so to 
        // get the "most" filtered disparity map available, on the 
        // basis of the selection of parameters by the user

        cv::Mat disp_vis = matcher->getDisparity("wls");

        if(!disp_vis.empty())
        {

#ifdef USE_GUI
            if(gui.toRefine())
           {
                orig = disp_vis.clone();

                if(!old_d.empty())
                    disp_vis = refineDisparity(old_d, disp_vis, gui.getRefineTh());

                old_d = orig.clone();
            }
#endif

            ImageOf<PixelMono> &outim = outDisp.prepare();

            outim = fromCvMat<PixelMono>(disp_vis);
            outDisp.write();
        }
    }

    // if the output port is active, computes the depth
    // map for the current disparity map (the filtered one,
    // in case the bilateral filtering is selected)

    if (outDepth.getOutputCount() > 0)
    {
        cv::Mat disp_depth = matcher->getDisparity16("wls");

        if (disp_depth.empty())
        {
            std::cout << "[DisparityModule] Impossible to compute the depth map.." << std::endl <<
                         "[DisparityModule] ..the disparity map is not available!" << std::endl;
        }
        else
        {

#ifdef USE_GUI
            if(gui.toRefine())
            {
                orig = disp_depth.clone();

                if(!old_de.empty())
                    disp_depth = refineDisparity(old_de, disp_depth, gui.getRefineTh());

                old_de = orig.clone();
            }
#endif

            outputDepth = this->depthFromDisparity(disp_depth, this->stereo->getQ(), this->stereo->getRLrect());

            ImageOf<PixelFloat> &outim = outDepth.prepare();

            outim = fromCvMat<PixelFloat>(outputDepth);

            outDepth.write();
        }
    }

    // if the GUI is used, handles the interaction
    // with it and updates its state

#ifdef USE_GUI
    if(this->debugWindow && !this->gui.isDone())
        this->gui.updateGUI();

    if(this->gui.isDone())
        this->gui.killGUI();
#endif

    return true;
}


double DispModule::getPeriod()
{
    // the updateModule() method gets synchronized
    // with camera input => no need for periodicity
    return 0.0;
}


bool DispModule::loadExtrinsics(yarp::os::ResourceFinder& rf, Mat& Ro, Mat& To, yarp::sig::Vector& eyes)
{
    Bottle extrinsics=rf.findGroup("STEREO_DISPARITY");

    eyes.resize(3,0.0);
    if (Bottle *bEyes=extrinsics.find("eyes").asList())
    {
        size_t sz=std::min(eyes.length(),(size_t)bEyes->size());
        for (size_t i=0; i<sz; i++)
            eyes[i]=bEyes->get(i).asFloat64();
    }

    std::cout << "[DisparityModule] Read Eyes Configuration = ("<<eyes.toString(3,3)<<")" << std::endl;

    if (Bottle *pXo=extrinsics.find("HN").asList())
    {
        Ro=Mat::zeros(3,3,CV_64FC1);
        To=Mat::zeros(3,1,CV_64FC1);
        for (int i=0; i<(pXo->size()-4); i+=4)
        {
            Ro.at<double>(i/4,0)=pXo->get(i).asFloat64();
            Ro.at<double>(i/4,1)=pXo->get(i+1).asFloat64();
            Ro.at<double>(i/4,2)=pXo->get(i+2).asFloat64();
            To.at<double>(i/4,0)=pXo->get(i+3).asFloat64();
        }
    }
    else
        return false;

    return true;
}


bool DispModule::loadConfigurationFile(yarp::os::ResourceFinder& rf, Mat& Ro, Mat& To, yarp::sig::Vector& eyes)
{

    Bottle extrinsics=rf.findGroup("STEREO_DISPARITY");

    // loads the eyes configuration from file

    eyes.resize(3,0.0);
    if (Bottle *bEyes=extrinsics.find("eyes").asList())
    {
        size_t sz = std::min(eyes.length(),(size_t)bEyes->size());
        for (size_t i=0; i<sz; i++)
            eyes[i] = bEyes->get(i).asFloat64();
    }

    std::cout << "[DisparityModule] Read Eyes Configuration = ("
         << eyes.toString(3,3) << ")" << std::endl;

    // loads the calibration matrix associated with the
    // stereo system

    if (Bottle *pXo=extrinsics.find("HN").asList())
    {
        Ro=Mat::zeros(3,3,CV_64FC1);
        To=Mat::zeros(3,1,CV_64FC1);
        for (int i=0; i<(pXo->size()-4); i+=4)
        {
            Ro.at<double>(i/4,0) = pXo->get(i).asFloat64();
            Ro.at<double>(i/4,1) = pXo->get(i+1).asFloat64();
            Ro.at<double>(i/4,2) = pXo->get(i+2).asFloat64();
            To.at<double>(i/4,0) = pXo->get(i+3).asFloat64();
        }
    }
    else
        return false;

    // loads the stereo parameters from the configuration file

    if (Bottle *pXo=extrinsics.find("params").asList())
    {

        this->stereo_parameters.minDisparity =        pXo->get(0).asInt32();
        this->useBestDisp =                           pXo->get(1).asBool();
        this->stereo_parameters.numberOfDisparities = pXo->get(2).asInt32();
        this->stereo_parameters.SADWindowSize =       pXo->get(3).asInt32();
        this->stereo_parameters.disp12MaxDiff =       pXo->get(4).asInt32();
        this->stereo_parameters.preFilterCap =        pXo->get(5).asInt32();
        this->stereo_parameters.uniquenessRatio =     pXo->get(6).asInt32();
        this->stereo_parameters.speckleWindowSize =   pXo->get(7).asInt32();
        this->stereo_parameters.speckleRange =        pXo->get(8).asInt32();

        this->stereo_parameters.sigmaColorBLF =       pXo->get(9).asFloat64();
        this->stereo_parameters.sigmaSpaceBLF =       pXo->get(10).asFloat64();

        this->stereo_parameters.wls_lambda =          pXo->get(11).asFloat64();
        this->stereo_parameters.wls_sigma =           pXo->get(12).asFloat64();

        this->stereo_parameters.stereo_matching =     static_cast<SM_MATCHING_ALG>(pXo->get(13).asInt32());
        this->stereo_parameters.BLFfiltering =        static_cast<SM_BLF_FILTER>(pXo->get(14).asInt32());
        this->stereo_parameters.WLSfiltering =        static_cast<SM_WLS_FILTER>(pXo->get(15).asInt32());

    }
    else
        this->initializeStereoParams();

    return true;
}


bool DispModule::loadIntrinsics(yarp::os::ResourceFinder &rf, Mat &KL, Mat &KR, Mat &DistL,
        Mat &DistR)
{
    Bottle left=rf.findGroup("CAMERA_CALIBRATION_LEFT");
    if(!left.check("fx") || !left.check("fy") || !left.check("cx") || !left.check("cy"))
        return false;

    double fx=left.find("fx").asFloat64();
    double fy=left.find("fy").asFloat64();

    double cx=left.find("cx").asFloat64();
    double cy=left.find("cy").asFloat64();

    double k1=left.check("k1",Value(0)).asFloat64();
    double k2=left.check("k2",Value(0)).asFloat64();

    double p1=left.check("p1",Value(0)).asFloat64();
    double p2=left.check("p2",Value(0)).asFloat64();

    DistL=Mat::zeros(1,8,CV_64FC1);
    DistL.at<double>(0,0)=k1;
    DistL.at<double>(0,1)=k2;
    DistL.at<double>(0,2)=p1;
    DistL.at<double>(0,3)=p2;

    KL=Mat::eye(3,3,CV_64FC1);
    KL.at<double>(0,0)=fx;
    KL.at<double>(0,2)=cx;
    KL.at<double>(1,1)=fy;
    KL.at<double>(1,2)=cy;

    Bottle right=rf.findGroup("CAMERA_CALIBRATION_RIGHT");
    if(!right.check("fx") || !right.check("fy") || !right.check("cx") || !right.check("cy"))
        return false;

    fx=right.find("fx").asFloat64();
    fy=right.find("fy").asFloat64();

    cx=right.find("cx").asFloat64();
    cy=right.find("cy").asFloat64();

    k1=right.check("k1",Value(0)).asFloat64();
    k2=right.check("k2",Value(0)).asFloat64();

    p1=right.check("p1",Value(0)).asFloat64();
    p2=right.check("p2",Value(0)).asFloat64();

    DistR=Mat::zeros(1,8,CV_64FC1);
    DistR.at<double>(0,0)=k1;
    DistR.at<double>(0,1)=k2;
    DistR.at<double>(0,2)=p1;
    DistR.at<double>(0,3)=p2;

    KR=Mat::eye(3,3,CV_64FC1);
    KR.at<double>(0,0)=fx;
    KR.at<double>(0,2)=cx;
    KR.at<double>(1,1)=fy;
    KR.at<double>(1,2)=cy;

    return true;
}



bool DispModule::updateExtrinsics(Mat& Rot, Mat& Tr, yarp::sig::Vector& eyes,
        const string& groupname)
{
    ofstream out;
    out.open(camCalibFile.c_str());
    if (out.is_open())
    {
        out << endl;
        out << "["+groupname+"]" << endl;
        out << "eyes (" << eyes.toString() << ")" << endl;
        out << "HN ("
            << Rot.at<double>(0,0) << " "
            << Rot.at<double>(0,1) << " "
            << Rot.at<double>(0,2) << " "
            << Tr.at<double>(0,0) << " "
            << Rot.at<double>(1,0) << " "
            << Rot.at<double>(1,1) << " "
            << Rot.at<double>(1,2) << " "
            << Tr.at<double>(1,0) << " "
            << Rot.at<double>(2,0) << " "
            << Rot.at<double>(2,1) << " "
            << Rot.at<double>(2,2) << " "
            << Tr.at<double>(2,0) << " "
            << 0.0 << " " << 0.0 << " " << 0.0 << " " << 1.0                << ")"
            << endl;

        out.close();
        return true;
    }
    else
        return false;
}


bool DispModule::updateConfigurationFile(Mat& Rot, Mat& Tr, yarp::sig::Vector& eyes,
        const string& groupname)
{
    ofstream out;
    out.open(camCalibFile.c_str());
    if (out.is_open())
    {
        out << endl;
        out << "["+groupname+"]" << endl;
        out << "eyes (" << eyes.toString() << ")" << endl;
        out << "HN ("
            << Rot.at<double>(0,0) << " "
            << Rot.at<double>(0,1) << " "
            << Rot.at<double>(0,2) << " "
            << Tr.at<double>(0,0) << " "
            << Rot.at<double>(1,0) << " "
            << Rot.at<double>(1,1) << " "
            << Rot.at<double>(1,2) << " "
            << Tr.at<double>(1,0) << " "
            << Rot.at<double>(2,0) << " "
            << Rot.at<double>(2,1) << " "
            << Rot.at<double>(2,2) << " "
            << Tr.at<double>(2,0) << " "
            << 0.0 << " " << 0.0 << " " << 0.0 << " " << 1.0                << ")"
            << endl;

        // TODO: change the following code and insert the stereo 
        // parameters in the corresponding struct as defined in StereoMatcher.h

        out << "params ("
            << this->stereo_parameters.minDisparity << " "
            << this->useBestDisp << " "
            << this->stereo_parameters.numberOfDisparities << " "
            << this->stereo_parameters.SADWindowSize << " "
            << this->stereo_parameters.disp12MaxDiff << " "
            << this->stereo_parameters.preFilterCap << " "
            << this->stereo_parameters.uniquenessRatio << " "
            << this->stereo_parameters.speckleWindowSize << " "
            << this->stereo_parameters.speckleRange << " "
            << this->stereo_parameters.sigmaColorBLF << " "
            << this->stereo_parameters.sigmaSpaceBLF << " "
            << this->stereo_parameters.wls_lambda << " "
            << this->stereo_parameters.wls_sigma << " "
            << this->stereo_parameters.stereo_matching << " "
            << this->stereo_parameters.BLFfiltering << " "
            << this->stereo_parameters.WLSfiltering << ")"
            << endl;

        out.close();
        return true;
    }
    else
        return false;
}


void DispModule::setDispParameters(bool _useBestDisp, int _uniquenessRatio,
        int _speckleWindowSize,int _speckleRange,
        int _numberOfDisparities, int _SADWindowSize,
        int _minDisparity, int _preFilterCap, int _disp12MaxDiff)
{
    this->mutexDisp.lock();

    this->useBestDisp=_useBestDisp;
    this->stereo_parameters.uniquenessRatio=_uniquenessRatio;
    this->stereo_parameters.speckleWindowSize=_speckleWindowSize;
    this->stereo_parameters.speckleRange=_speckleRange;
    this->stereo_parameters.numberOfDisparities=_numberOfDisparities;
    this->stereo_parameters.SADWindowSize=_SADWindowSize;
    this->stereo_parameters.minDisparity=_minDisparity;
    this->stereo_parameters.preFilterCap=_preFilterCap;
    this->stereo_parameters.disp12MaxDiff=_disp12MaxDiff;

    this->mutexDisp.unlock();

}


Point3f DispModule::get3DPoints(int u, int v, const string &drive)
{
    Point3f point(0.0f,0.0f,0.0f);
    if ((drive!="RIGHT") && (drive!="LEFT") && (drive!="ROOT"))
        return point;

    std::lock_guard<std::mutex> lg(mutexDisp);

    // Mapping from Rectified Cameras to Original Cameras
    const Mat& Mapper=this->stereo->getMapperL();
    if (Mapper.empty())
        return point;

    float usign=Mapper.ptr<float>(v)[2*u];
    float vsign=Mapper.ptr<float>(v)[2*u+1];

    u=cvRound(usign);
    v=cvRound(vsign);

    const Mat& disp16m=this->matcher->getDisparity16("wls");
    if (disp16m.empty() || (u<0) || (u>=disp16m.cols) || (v<0) || (v>=disp16m.rows))
        return point;

    const Mat& Q=this->stereo->getQ();
    IplImage disp16=disp16m;
    CvScalar scal=cvGet2D(&disp16,v,u);
    double disparity=scal.val[0]/16.0;
    float w=(float)(disparity*Q.at<double>(3,2)+Q.at<double>(3,3));
    point.x=(float)((usign+1)*Q.at<double>(0,0)+Q.at<double>(0,3));
    point.y=(float)((vsign+1)*Q.at<double>(1,1)+Q.at<double>(1,3));
    point.z=(float)Q.at<double>(2,3);

    point.x/=w;
    point.y/=w;
    point.z/=w;

    // discard points far more than 10 meters or with not valid disparity (<0)

    // TODO: "discards" means it returns the point in the same way?

    if ((point.z>10.0f) || (point.z<0.0f))
        return point;

    if (drive=="ROOT")
    {
        const Mat& RLrect=this->stereo->getRLrect().t();
        Mat Tfake=Mat::zeros(0,3,CV_64F);
        Mat P(4,1,CV_64FC1);
        P.at<double>(0,0)=point.x;
        P.at<double>(1,0)=point.y;
        P.at<double>(2,0)=point.z;
        P.at<double>(3,0)=1.0;

        Mat Hrect=buildRotTras(RLrect,Tfake);
        P=HL_root*Hrect*P;
        point.x=(float)(P.at<double>(0,0)/P.at<double>(3,0));
        point.y=(float)(P.at<double>(1,0)/P.at<double>(3,0));
        point.z=(float)(P.at<double>(2,0)/P.at<double>(3,0));
    }
    else if (drive=="LEFT")
    {
        Mat P(3,1,CV_64FC1);
        P.at<double>(0,0)=point.x;
        P.at<double>(1,0)=point.y;
        P.at<double>(2,0)=point.z;

        P=this->stereo->getRLrect().t()*P;
        point.x=(float)P.at<double>(0,0);
        point.y=(float)P.at<double>(1,0);
        point.z=(float)P.at<double>(2,0);
    }
    else if (drive=="RIGHT")
    {
        const Mat& Rright=this->stereo->getRotation();
        const Mat& Tright=this->stereo->getTranslation();
        const Mat& RRright=this->stereo->getRRrect().t();
        Mat TRright=Mat::zeros(0,3,CV_64F);

        Mat HRL=buildRotTras(Rright,Tright);
        Mat Hrect=buildRotTras(RRright,TRright);

        Mat P(4,1,CV_64FC1);
        P.at<double>(0,0)=point.x;
        P.at<double>(1,0)=point.y;
        P.at<double>(2,0)=point.z;
        P.at<double>(3,0)=1.0;

        P=Hrect*HRL*P;
        point.x=(float)(P.at<double>(0,0)/P.at<double>(3,0));
        point.y=(float)(P.at<double>(1,0)/P.at<double>(3,0));
        point.z=(float)(P.at<double>(2,0)/P.at<double>(3,0));
    }

    return point;
}


Mat DispModule::buildRotTras(const Mat& R, const Mat& T)
{     
    Mat A=Mat::eye(4,4,CV_64F);
    for (int i=0; i<R.rows; i++)
    {
        double* Mi=A.ptr<double>(i);
        const double* MRi=R.ptr<double>(i);
        for (int j=0; j<R.cols; j++)
            Mi[j]=MRi[j];
    }

    for (int i=0; i<T.rows; i++)
    {
        double* Mi=A.ptr<double>(i);
        const double* MRi=T.ptr<double>(i);
        Mi[3]=MRi[0];
    }

    return A;
}



Matrix DispModule::getCameraHGazeCtrl(int camera)
{
    yarp::sig::Vector x_curr;
    yarp::sig::Vector o_curr;
    bool check=false;
    if(camera==LEFT)
        check=igaze->getLeftEyePose(x_curr, o_curr);
    else
        check=igaze->getRightEyePose(x_curr, o_curr);

    if(!check)
    {
        Matrix H_curr(4, 4);
        return H_curr;
    }

    Matrix R_curr=axis2dcm(o_curr);
    Matrix H_curr=R_curr;
    H_curr.setSubcol(x_curr,0,3);

    if(camera==LEFT)
    {
        mutexDisp.lock();
        convert(H_curr,HL_root);
        mutexDisp.unlock();
    }
    else if(camera==RIGHT)
    {
        mutexDisp.lock();
        convert(H_curr,HR_root);
        mutexDisp.unlock();
    }

    return H_curr;
}



void DispModule::convert(Matrix& matrix, Mat& mat)
{
    mat=cv::Mat(matrix.rows(),matrix.cols(),CV_64FC1);
    for(int i=0; i<matrix.rows(); i++)
        for(int j=0; j<matrix.cols(); j++)
            mat.at<double>(i,j)=matrix(i,j);
}



void DispModule::convert(Mat& mat, Matrix& matrix)
{
    matrix.resize(mat.rows,mat.cols);
    for(int i=0; i<mat.rows; i++)
        for(int j=0; j<mat.cols; j++)
            matrix(i,j)=mat.at<double>(i,j);
}



bool DispModule::respond(const Bottle& command, Bottle& reply)
{
    if(command.size()==0)
        return false;

    if (command.get(0).asString()=="quit") {
        std::cout << "[DisparityModule] Closing..." << std::endl;
        return false;
    }

    if (command.get(0).asString()=="help") {
        reply.addVocab32("many");
        reply.addString("Available commands are:");
        reply.addString("- [calibrate]: It recomputes the camera positions once.");
        reply.addString("- [save]: It saves the current camera positions and uses it when the module starts.");
        reply.addString("- [getH]: It returns the calibrated stereo matrix.");
        reply.addString("- [setNumDisp NumOfDisparities]: It sets the expected number of disparity (in pixel). Values must be divisible by 32. ");
        reply.addString("- [Point x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the LEFT eye.");
        reply.addString("- [x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z ur vr computed using the depth map wrt the the ROOT reference system.(ur vr) is the corresponding pixel in the Right image. ");
        reply.addString("- [Left x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the LEFT eye. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0). ");
        reply.addString("- [Right x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the RIGHT eye. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0).");
        reply.addString("- [Root x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the ROOT reference system. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0).");
        reply.addString("- [Rect tlx tly w h step]: Given the pixels in the rectangle defined by {(tlx,tly) (tlx+w,tly+h)} (parsed by columns), the response contains the corresponding 3D points in the ROOT frame. The optional parameter step defines the sampling quantum; by default step=1.");
        reply.addString("- [Points u_1 v_1 ... u_n v_n]: Given a list of n pixels, the response contains the corresponding 3D points in the ROOT frame.");
        reply.addString("- [Flood3D x y dist]: Perform 3D flood-fill on the seed point (x,y), returning the following info: [u_1 v_1 x_1 y_1 z_1 ...]. The optional parameter dist expressed in meters regulates the fill (by default = 0.004).");
        reply.addString("- [uL_1 vL_1 uR_1 vR_1 ... uL_n vL_n uR_n vR_n]: Given n quadruples uL_i vL_i uR_i vR_i, where uL_i vL_i are the pixel coordinates in the Left image and uR_i vR_i are the coordinates of the matched pixel in the Right image, the response is a set of 3D points (X1 Y1 Z1 ... Xn Yn Zn) wrt the ROOT reference system.");
        reply.addString("- [cart2stereo X Y Z]: Given a world point X Y Z wrt to ROOT reference frame the response is the projection (uL vL uR vR) in the Left and Right images.");
        reply.addString("- [doBLF flag]: activate Bilateral filter for flag = true, and skip it for flag = false.");
        reply.addString("- [bilatfilt sigmaColor sigmaSpace]: Set the parameters for the bilateral filer (default sigmaColor = 10.0, sigmaSpace = 10.0).");
        reply.addString("- [algorithm name]: Set the stereo matching algorithm used (default = SGMB(cpu)).");
        reply.addString("For more details on the commands, check the module's documentation");
        return true;
    }

    if (command.get(0).asString()=="calibrate")
    {
        mutexRecalibration.lock();
        numberOfTrials=0;
        this->runRecalibration=true;
        mutexRecalibration.unlock();

        calibEndEvent.reset();
        calibEndEvent.wait();

        if (this->calibUpdated)
        {
            R0=this->stereo->getRotation();
            T0=this->stereo->getTranslation();
            eyes0=eyes;

            reply.addString("ACK");
        }
        else
            reply.addString("[DisparityModule] Calibration failed after 5 trials.. Please show a non planar scene.");

        return true;
    }

    if (command.get(0).asString()=="save")
    {
        updateExtrinsics(R0,T0,eyes0,"STEREO_DISPARITY");
        reply.addString("ACK");
        return true;
    }

    if (command.get(0).asString()=="getH")
    {
        Mat RT0=buildRotTras(R0,T0);
        Matrix H0; convert(RT0,H0);

        reply.read(H0);
        return true;
    }

    if (command.get(0).asString()=="setNumDisp")
    {
        int dispNum=command.get(1).asInt32();
        if(dispNum%32==0)
        {
            this->stereo_parameters.numberOfDisparities=dispNum;
            this->setDispParameters(useBestDisp,
                                    this->stereo_parameters.uniquenessRatio,
                                    this->stereo_parameters.speckleWindowSize,
                                    this->stereo_parameters.speckleRange,
                                    this->stereo_parameters.numberOfDisparities,
                                    this->stereo_parameters.SADWindowSize,
                                    this->stereo_parameters.minDisparity,
                                    this->stereo_parameters.preFilterCap,
                                    this->stereo_parameters.disp12MaxDiff);
            reply.addString("ACK");
            return true;
        }
        else
        {
            reply.addString("Num Disparity must be divisible by 32");
            return true;
        }
    }

    if (command.get(0).asString()=="setMinDisp")
    {
        int dispNum=command.get(1).asInt32();
        this->stereo_parameters.minDisparity=dispNum;
        this->setDispParameters(useBestDisp,
                                this->stereo_parameters.uniquenessRatio,
                                this->stereo_parameters.speckleWindowSize,
                                this->stereo_parameters.speckleRange,
                                this->stereo_parameters.numberOfDisparities,
                                this->stereo_parameters.SADWindowSize,
                                this->stereo_parameters.minDisparity,
                                this->stereo_parameters.preFilterCap,
                                this->stereo_parameters.disp12MaxDiff);
        reply.addString("ACK");
        return true;
    }

    if (command.get(0).asString()=="set" && command.size()==10)
    {
        bool useBestDisp=command.get(1).asInt32() ? true : false;
        int uniquenessRatio=command.get(2).asInt32();
        int speckleWindowSize=command.get(3).asInt32();
        int speckleRange=command.get(4).asInt32();
        int numberOfDisparities=command.get(5).asInt32();
        int SADWindowSize=command.get(6).asInt32();
        int minDisparity=command.get(7).asInt32();
        int preFilterCap=command.get(8).asInt32();
        int disp12MaxDiff=command.get(9).asInt32();

        this->setDispParameters(useBestDisp,
                                uniquenessRatio,
                                speckleWindowSize,
                                speckleRange,
                                numberOfDisparities,
                                SADWindowSize,
                                minDisparity,
                                preFilterCap,
                                disp12MaxDiff);

        reply.addString("ACK");
    }
    else if (command.get(0).asString()=="Point" || command.get(0).asString()=="Left" )
    {
        int u = command.get(1).asInt32();
        int v = command.get(2).asInt32();
        Point3f point = this->get3DPoints(u,v);
        reply.addFloat64(point.x);
        reply.addFloat64(point.y);
        reply.addFloat64(point.z);
    }
    else if (command.get(0).asString()=="Right")
    {
        int u = command.get(1).asInt32();
        int v = command.get(2).asInt32();
        Point3f point = this->get3DPoints(u,v,"RIGHT");
        reply.addFloat64(point.x);
        reply.addFloat64(point.y);
        reply.addFloat64(point.z);
    }
    else if (command.get(0).asString()=="Root")
    {
        int u = command.get(1).asInt32();
        int v = command.get(2).asInt32();
        Point3f point = this->get3DPoints(u,v,"ROOT");
        reply.addFloat64(point.x);
        reply.addFloat64(point.y);
        reply.addFloat64(point.z);
    }
    else if (command.get(0).asString()=="Rect")
    {
        int tl_u = command.get(1).asInt32();
        int tl_v = command.get(2).asInt32();
        int br_u = tl_u+command.get(3).asInt32();
        int br_v = tl_v+command.get(4).asInt32();

        int step = 1;
        if (command.size()>=6)
            step=command.get(5).asInt32();

        for (int u=tl_u; u<br_u; u+=step)
        {
            for (int v=tl_v; v<br_v; v+=step)
            {
                Point3f point=this->get3DPoints(u,v,"ROOT");
                reply.addFloat64(point.x);
                reply.addFloat64(point.y);
                reply.addFloat64(point.z);
            }
        }
    }
    else if (command.get(0).asString()=="Points")
    {
        for (int cnt=1; cnt<command.size()-1; cnt+=2)
        {
            int u=command.get(cnt).asInt32();
            int v=command.get(cnt+1).asInt32();
            Point3f point=this->get3DPoints(u,v,"ROOT");
            reply.addFloat64(point.x);
            reply.addFloat64(point.y);
            reply.addFloat64(point.z);
        }
    }
    else if (command.get(0).asString()=="cart2stereo")
    {
        double x = command.get(1).asFloat64();
        double y = command.get(2).asFloat64();
        double z = command.get(3).asFloat64();

        Point2f pointL = this->projectPoint("left",x,y,z);
        Point2f pointR = this->projectPoint("right",x,y,z);

        reply.addFloat64(pointL.x);
        reply.addFloat64(pointL.y);
        reply.addFloat64(pointR.x);
        reply.addFloat64(pointR.y);
    }
    else if (command.get(0).asString()=="bilatfilt" && command.size()==3)
    {
        if (!this->doBLF){
            this->doBLF = true;
            reply.addString("Bilateral filter activated.");
        }
        this->stereo_parameters.sigmaColorBLF = command.get(1).asFloat64();
        this->stereo_parameters.sigmaSpaceBLF = command.get(2).asFloat64();
        reply.addString("BLF sigmaColor ");
        reply.addFloat64(this->stereo_parameters.sigmaColorBLF);
        reply.addString("BLF sigmaSpace ");
        reply.addFloat64(this->stereo_parameters.sigmaSpaceBLF);
    }
    else if (command.get(0).asString()=="doBLF")
    {
        bool onoffBLF = command.get(1).asBool();
        if (onoffBLF == false ){     // turn OFF Bilateral Filtering
            if (this->doBLF == true){
                this->doBLF = false;
                reply.addString("Bilateral Filter OFF");
            } else {
                reply.addString("Bilateral Filter already OFF");
            }

        } else {                    // turn ON Bilateral Filtering
            if (this->doBLF == true){
                reply.addString("Bilateral Filter Already Running");
            } else {                                     // Set any different from 0 to activate bilateral filter.
                this->doBLF = true;
                reply.addString("Bilateral Filter ON");
            }
        }
        reply.addFloat64(this->stereo_parameters.sigmaColorBLF);
        reply.addFloat64(this->stereo_parameters.sigmaSpaceBLF);
    }
    else
        reply.addString("NACK");

    return true;
}


Point2f DispModule::projectPoint(const string &camera, double x, double y, double z)
{
    Point3f point3D;
    point3D.x=(float)x;
    point3D.y=(float)y;
    point3D.z=(float)z;

    vector<Point3f> points3D;

    points3D.push_back(point3D);

    vector<Point2f> response;

    mutexDisp.lock();

    if(camera=="left")
        response=this->stereo->projectPoints3D("left",points3D,HL_root);
    else
        response=this->stereo->projectPoints3D("right",points3D,HL_root); // this should be HR_root

    mutexDisp.unlock();

    return response[0];
}


cv::Mat DispModule::refineDisparity(cv::Mat old_disp, cv::Mat new_disp, int th)
{
    cv::Mat mask, result;

    cv::absdiff(old_disp, new_disp, mask);
    cv::threshold(mask, mask, th, 1, cv::THRESH_BINARY_INV);

    result = new_disp.mul(mask);

    return result;
}


DispModule::~DispModule()
{

#ifdef USE_GUI
    if(this->debugWindow)
        this->gui.killGUI();
#endif

}


DispModule::DispModule()
{
    this->debugWindow = false;
}


int main(int argc, char *argv[])
{
    Network yarp;

    if (!yarp.checkNetwork())
        return 1;

    // rough quick check of the command line parameters, 
    // has to be definitely made in a smart way

    ResourceFinder rf;
    rf.setDefaultContext("cameraCalibration");
    rf.configure(argc, argv);

    if(rf.check("use640"))
        rf.setDefaultConfigFile("icubEyes_640x480.ini");
    else
        rf.setDefaultConfigFile("icubEyes.ini");

    rf.configure(argc, argv);

    DispModule mod;
    
    return mod.runModule(rf);
}