DispModule.h

/*
 * 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 <cstdio>
#include <string>
#include <iostream>
#include <fstream>
#include <set>
#include <deque>
#include <mutex>
 
#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/sig/all.h>
#include <yarp/math/Math.h>
 
#include <iCub/ctrl/math.h>
#include <iCub/iKin/iKinFwd.h>
#include <iCub/stereoVision/stereoCamera.h>
 
 
#ifdef USE_GUI
    #include "cvgui.h"
#endif
 
#include "common.h"
#include "StereoMatcher.h"
 
#ifdef USING_GPU
    #include <iCub/stereoVision/utils.h>
#endif
 
#define LEFT    0
#define RIGHT   1
 
using namespace std;
using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace yarp::math;
using namespace iCub::ctrl;
using namespace iCub::iKin;
 
using namespace cv::ximgproc;
 
class DispModule: public yarp::os::RFModule
{
    StereoCamera* stereo;
    Mat outputDm, outputDepth;
    Mat leftMat, rightMat;
 
#ifdef USING_GPU
    /* pointer to the utilities class */
    Utilities *utils;
#endif
 
    // input YARP ports
    yarp::os::Port rpc;
    yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > leftImgPort;
    yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > rightImgPort;
    Port handlerPort;
 
    // output YARP ports
    BufferedPort<ImageOf<PixelFloat> > outDepth;
    BufferedPort<ImageOf<PixelMono> >  outDisp;
 
    // number of trials to consider when recalibrating the system
    int numberOfTrials;
 
    // path of the camCalib configuration file 
    string camCalibFile;
 
    // stereo matching and calibration auxiliary parameters
    bool useBestDisp;
    bool runRecalibration;
    bool calibUpdated;
    bool debugWindow;
    bool doBLF;
 
    // structures meant to hold the stereo parameters
    Params stereo_parameters, original_parameters;
 
    // mutex and other handles needed by the module
    std::mutex mutexRecalibration;
    Event calibEndEvent;
    std::mutex mutexDisp;
 
    ResourceFinder localCalibration;
 
    // objects to interface with the robot's state
    PolyDriver headCtrl,gazeCtrl;
    IEncoders* iencs;
    IGazeControl* igaze;
    yarp::sig::Vector eyes0,eyes;
    int nHeadAxes;
    Mat HL_root;
    Mat HR_root;
    Mat R0,T0;
 
    // objects used in the refinement of the disparity map
    cv::Mat old_d, old_de;
    cv::Mat orig;
 
    bool init;
 
    // parameters for the CUDA implementation of SGBM
    SGM_PARAMS cuda_params, params_right;
 
    // object handling the stereo matching algorithms
    StereoMatcherNew * matcher;
 
    bool loadIntrinsics(yarp::os::ResourceFinder &rf, Mat &KL, Mat &KR, Mat &DistL, Mat &DistR);
 
 
    bool loadConfigurationFile(yarp::os::ResourceFinder& rf, Mat& Ro, Mat& To, yarp::sig::Vector& eyes);
 
    // bool loadStereoParameters(yarp::os::ResourceFinder& rf, Mat& Ro, Mat& To, yarp::sig::Vector& eyes);
 
    Mat buildRotTras(const Mat& R, const Mat& T);
 
    Matrix getCameraHGazeCtrl(int camera);
 
    void convert(Matrix& matrix, Mat& mat);
 
    void convert(Mat& mat, Matrix& matrix);
 
    bool loadExtrinsics(yarp::os::ResourceFinder& rf, Mat& Ro, Mat& To, yarp::sig::Vector& eyes);
 
    bool updateExtrinsics(Mat& Rot, Mat& Tr, yarp::sig::Vector& eyes, const string& groupname);
 
    bool updateConfigurationFile(Mat& Rot, Mat& Tr, yarp::sig::Vector& eyes, const string& groupname);
 
    void updateViaGazeCtrl(const bool update);
 
    void updateViaKinematics(const yarp::sig::Vector& deyes);
 
    void initializeStereoParams();
 
    Mat depthFromDisparity(Mat disp, Mat Q, Mat R);
 
    void handleGuiUpdate();
 
    void recalibrate();
 
#ifdef USE_GUI
    GUI gui;
#endif
 
public:
 
    // module methods
 
    bool configure(ResourceFinder &rf);
    bool close();
    bool updateModule();
    double getPeriod();
    bool interruptModule();
    bool respond(const Bottle& command, Bottle& reply);
 
    void setDispParameters(bool _useBestDisp, int _uniquenessRatio, int _speckleWindowSize,
                           int _speckleRange, int _numberOfDisparities, int _SADWindowSize,
                           int _minDisparity, int _preFilterCap, int _disp12MaxDiff);
 
    Point3f get3DPoints(int u, int v, const string &drive="LEFT");
 
    Point2f projectPoint(const string &camera, double x, double y, double z);
 
    DispModule();
    ~DispModule();
 
    void printParameters();
 
    cv::Mat refineDisparity(cv::Mat old_disp, cv::Mat new_disp, int th);
 
};