SFM.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 <mutex>
#include <condition_variable>
#include <string>
#include <iostream>
#include <fstream>
#include <set>
#include <deque>
 
#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>
 
#include "fastBilateral.hpp"
 
#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;
 
 
class SFM: public yarp::os::RFModule
{
    StereoCamera* stereo;
    Mat           outputDm;
    Mat leftMat, rightMat;
 
#ifdef USING_GPU
    /* pointer to the utilities class */
    Utilities *utils;
#endif
 
    yarp::os::Port rpc;
    yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > leftImgPort;
    yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > rightImgPort;
    BufferedPort<ImageOf<PixelRgbFloat> > worldCartPort;
    BufferedPort<ImageOf<PixelRgbFloat> > worldCylPort;
    Port handlerPort;
 
    BufferedPort<ImageOf<PixelMono> > outDisp;
    BufferedPort<ImageOf<PixelBgr> >  outMatch;
 
    BufferedPort<ImageOf<PixelRgb> >  outLeftRectImgPort;
    BufferedPort<ImageOf<PixelRgb> >  outRightRectImgPort;
 
    int numberOfTrials;
    string camCalibFile;
    bool useBestDisp;
    int uniquenessRatio;
    int speckleWindowSize;
    int speckleRange;
    int numberOfDisparities;
    int SADWindowSize;
    int minDisparity;
    int preFilterCap;
    int disp12MaxDiff;
    bool doSFM;
    bool calibUpdated;
    double sigmaColorBLF;
    double sigmaSpaceBLF;
    bool doBLF;
    mutex mutexRecalibration;
    mutex mutexDisp;
    mutex mtx_calibEndEvent;
    condition_variable cv_calibEndEvent;
 
    PolyDriver headCtrl,gazeCtrl;
    IEncoders* iencs;
    IGazeControl* igaze;
    yarp::sig::Vector eyes0,eyes;
    int nHeadAxes;
    Mat HL_root;
    Mat HR_root;
    Mat R0,T0;
 
    bool loadIntrinsics(yarp::os::ResourceFinder &rf, Mat &KL, Mat &KR, Mat &DistL, Mat &DistR);
    Mat buildRotTras(const Mat& R, const Mat& T);
    Matrix getCameraHGazeCtrl(int camera);
    void convert(Matrix& matrix, Mat& mat);
    void convert(Mat& mat, Matrix& matrix);
    void fillWorld3D(ImageOf<PixelRgbFloat> &worldCartImg, ImageOf<PixelRgbFloat> &worldCylImg);
    void floodFill(const Point &seed,const Point3f &p0, const double dist, set<int> &visited, Bottle &res);
    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);
    void updateViaGazeCtrl(const bool update);
    void updateViaKinematics(const yarp::sig::Vector& deyes);
    bool init;
 
public:
 
    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");
    Point3f get3DPointsAndDisp(int u, int v, int &uR, int &vR, const string &drive);
 
    Point3f get3DPointMatch(double u1, double v1, double u2, double v2, const string &drive="LEFT");
    Point2f projectPoint(const string &camera, double x, double y, double z);
};