stereoCamera.h

/* 
 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
 * 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
*/
 
#ifndef _ICUB_STEREOVISION_STEREOCAMERA_H_
#define _ICUB_STEREOVISION_STEREOCAMERA_H_
 
#include <iostream>
#include <fstream>
#include <mutex>
#include <string>
#include <vector>
 
#include <opencv2/opencv.hpp>
 
#include <iCub/stereoVision/camera.h>
#include <iCub/stereoVision/elasWrapper.h>
 
#include <yarp/os/all.h>
 
//using namespace cv::ximgproc;
 
#define LEFT    0
#define RIGHT   1
 
using namespace std;
using namespace yarp::os;
 
 
using cv::Mat;
using cv::Point2f;
using cv::Point3f;
using cv::Size;
using cv::Ptr;
using cv::FileStorage;
using cv::DMatch;
using cv::FileNode;
using cv::FeatureDetector;
using cv::DescriptorMatcher;
using cv::KeyPoint;
using cv::StereoSGBM;
using cv::StereoBM;
using cv::Rect;
using cv::SVD;
using cv::Vec3f;
using cv::Range;
using cv::Scalar;
using cv::TermCriteria;
class StereoCamera 
{
private:
    // stored Images
    Mat imleft; // Left Image
    Mat imleftund; // Undistorted Image
    Mat imright; // Right Image
    Mat imrightund; // Undistorted Image Right
    Mat Disparity; // Disparity Map Image
    Mat Disparity16; // Disparity 16 Bit Signed
    Mat imgLeftRect; // Rectified Left Image
    Mat imgRightRect; // Rectified Right Image
 
    // stored Matrices
    Mat Pleft; // Camera Matrix Left 3x4
    Mat Pright; // Camera Matrix Right 3x4
    Mat Kleft; // Intrinsic Parameters Left 3x3
    Mat Kright; // Intrinsic Parameters Right 3x3
    Mat DistL; // Distortion Coefficients Left 4x1
    Mat DistR; // Distortion Coefficients Right 4x1
    Mat Rinit; // Initial Rotation 3x3
    Mat Tinit; // Initial Translation 3x3
    Mat R; // Rotation from Left to Right 3x3
    Mat T; // Translation from Left to Right 3x1
    Mat R_exp; // Expected Rotation
    Mat T_exp; // Expected Trans
    Mat Pleft_exp; // Expected Camera Matrix left
    Mat Pright_exp; // Expected Camera Matrix right
    Mat Q; // Depth Matrix 4x4
    Mat F; // Fundamental Matrix 3x3
    Mat E; // Essential Matrix 3x3
    Mat RLrect; // Rotation from Left Camera to Rectified Left Camera 3x3
    Mat RRrect; // Rotation from Right Camera to Rectified Right Camera 3x3
    Mat PLrect; // Rectified Left Camera Matrix
    Mat PRrect; // Rectified Right Camera Matrix
    Mat map11; //Mapping from from rectified to original
    Mat map12; //Mapping from from rectified to original
    Mat map21; //Mapping from from rectified to original
    Mat map22; //Mapping from from rectified to original
    Mat mapxL; //Mapping from Undistorted to Original Left image for the x coordinates
    Mat mapyL; //Mapping from Undistorted to Original Left image for the y coordinates
    Mat mapxR; //Mapping from Undistorted to Original Right image for the x coordinates
    Mat mapyR; //Mapping from Undistorted to Original Right image for the y coordinates
 
    Mat MapperL; // pixels mapping from original left camera to rectified left camera
    Mat MapperR; // pixels mapping from original right camera to rectified right camera
 
    vector<Point2f> PointsL; // Match Left
    vector<Point2f> PointsR; // Match Right
  
    vector<Point2f> InliersL; // Inliers Left
    vector<Point2f> InliersR; // Inliers Right
    
    double epipolarTh; //threshold for the constraint x'Fx=0 -> x'Fx<epipolarTh
 
    mutex mtx;
    bool cameraChanged;
    bool rectify;
    bool readStringList( const string& filename, vector<string>& l );
    void runStereoCalib(const vector<string>& imagelist, Size boardSize,float sqsize);
    double* reprojectionError(Mat& Rot, Mat& Tras);
    void crossCheckMatching(  cv::BFMatcher &descriptorMatcher, const Mat& descriptors1, const Mat& descriptors2, vector<DMatch>& filteredMatches12, double radius);
    void updatePMatrix();
    void stereoCalibration(string imagesFilePath, int boardWidth, int boardHeight,float sqsize);
    void normalizePoints(Mat & K1, Mat & K2, vector<Point2f> & PointsL, vector<Point2f> & PointsR);
    void getRotation(Mat & q, Mat & R);
    void printMatrix(Mat &matrix);
    double reprojectionErrorAvg();
    void savePoints(string pointsLPath, string pointsRPath, vector<Point2f>  PointL, vector<Point2f>  PointR);
 
    Mat buildRotTras(Mat & R, Mat & T);
    void buildUndistortRemap();
    void printStereoIntrinsic();
    void printExtrinsic();
    bool loadStereoParameters(yarp::os::ResourceFinder &rf, Mat &KL, Mat &KR, Mat &DistL, Mat &DistR, Mat &Ro, Mat &T);
    void updateExpectedCameraMatrices();
 
 
 
    bool use_elas;
    elasWrapper*  elaswrap;
 
    // DEBUG
    int debug_timings[10];
    int debug_count;
 
    cv::Mat right_disp, right_disp16;
 
 
 
public:
 
    Mat filtered_disp;
 
    StereoCamera(bool rectify=true);
 
    ~StereoCamera() { }
 
    StereoCamera(yarp::os::ResourceFinder &rf,bool rectify=true);
 
    StereoCamera(Camera First, Camera Second,bool rectify=true);
 
    void initELAS(yarp::os::ResourceFinder &rf);
 
    void stereoCalibration(vector<string> imageList, int boardWidth, int boardHeight, float sqsize=1.0);
 
 
    void saveCalibration(string extrinsicFilePath, string intrinsicFilePath);
 
    void setImages(const Mat &firstImg, const Mat &secondImg);
 
    cv::Mat findMatch(bool visualize=false, double displacement=20.0, double radius=200.0);
 
    void computeDisparity(bool best=true, int uniquenessRatio=15, int speckleWindowSize=50,int speckleRange=16, int numberOfDisparities=64, int SADWindowSize=7, int minDisparity=0, int preFilterCap=63, int disp12MaxDiff=0);
 
    void undistortImages();
 
    void horn(Mat &K1,Mat &K2, vector<Point2f> &Points1,vector<Point2f> &Points2,Mat &Rot,Mat &Tras);
 
    void hornRelativeOrientations(); 
 
    Point3f triangulation(Point2f& point1, Point2f& point2);
 
    Point3f triangulation(Point2f& point1, Point2f& point2, Mat Camera1, Mat Camera2);     
    
    Point3f triangulationLS(Point2f& point1, Point2f& point2, Mat Camera1, Mat Camera2); 
 
    Point3f metricTriangulation(Point2f &point1,double thMeters=10);
 
    Point3f metricTriangulation(Point2f &point1, Mat &H, double thMeters=10);
 
    Point3f triangulateKnownDisparity(float u, float v, float d, Mat &H);
 
    void estimateEssential();
 
    bool essentialDecomposition();
 
    void chierality( Mat& R1,  Mat& R2,  Mat& t1,  Mat& t2, Mat& R, Mat& t, vector<Point2f> points1, vector<Point2f> points2);
 
    const Mat& getImLeft() const;
 
    const Mat& getImRight() const;
 
    const Mat& getImLeftUnd() const;
 
    const Mat& getImRightUnd() const;
 
    const Mat& getDisparity() const;
 
    const Mat& getDisparity16() const;
 
    const Mat& getQ() const;
 
    const Mat& getKleft() const;
 
    const Mat& getKright() const;
 
    const Mat& getFundamental() const;
 
    const vector<Point2f>& getMatchLeft() const;
 
    const vector<Point2f>& getMatchRight() const;
 
    const Mat& getTranslation() const;
 
    const Mat& getRotation() const;
 
    const Mat& getMapperL() const;
 
    const Mat& getMapperR() const;
 
    const Mat& getRLrect() const;
 
    const Mat& getRRrect() const;
 
    void setRotation(Mat& Rot, int mode=0);
 
    void setTranslation(Mat& Tras, int mul=0);
 
    void setIntrinsics(Mat& K1, Mat& K2, Mat& Dist1, Mat& Dist2);
 
    void rectifyImages();
 
    const Mat& getLRectified() const;
 
    const Mat& getRRectified() const;
 
    vector<Point2f> projectPoints3D(string camera, vector<Point3f> &points3D, Mat &H);
 
    Mat computeWorldImage(Mat &H);
 
    const Mat& getDistCoeffRight() const;
 
    const Mat& getDistCoeffLeft() const;
 
    Point2f getDistortedPixel(int u, int v, int cam=1);
 
    Mat drawMatches();
    
    void setMatches(std::vector<cv::Point2f> & pointsL, std::vector<cv::Point2f> & pointsR); 
  
    void setExpectedPosition(Mat &Rot, Mat &Tran);
 
    Mat FfromP(Mat& P1, Mat& P2);
 
    Point2f fromRectifiedToOriginal(int u, int v, int camera);
 
    cv::Mat remapDisparity(cv::Mat disp);
 
    void updateMappings();
 
};
 
#endif // _ICUB_STEREOVISION_STEREOCAMERA_H_