双目相机去畸变和极线平行操作

class StereoMatch
{
public:
    StereoMatch(void);
    virtual ~StereoMatch(void);

    int init(int imgWidth, int imgHeight, const char* xmlFilePath);

    int bmMatch(cv::Mat& frameLeft, cv::Mat& frameRight, cv::Mat& disparity, cv::Mat& imageLeft, cv::Mat& imageRight,
                cv::Mat &cropLeft, cv::Mat &cropRight);
    cv::Ptr<cv::StereoBM> m_BM = cv::StereoBM::create();
    double          m_FL;

private:

    int loadCalibData(const char* xmlFilePath);

    bool   m_Calib_Data_Loaded;
    cv::Mat m_Calib_Mat_Q;
    cv::Mat m_Calib_Mat_Remap_X_L;
    cv::Mat m_Calib_Mat_Remap_Y_L;
    cv::Mat m_Calib_Mat_Remap_X_R;
    cv::Mat m_Calib_Mat_Remap_Y_R;
    cv::Mat m_Calib_Mat_Mask_Roi;
    cv::Rect m_Calib_Roi_L;
    cv::Rect m_Calib_Roi_R;

    int m_frameWidth;
    int m_frameHeight;
    int m_numberOfDisparies;

    int m_nViewWidth;
    int m_nViewHeight;
    int m_nViewDepth;
    cv::Rect m_rect;

};



StereoMatch::StereoMatch(void)
        : m_frameWidth(0), m_frameHeight(0), m_numberOfDisparies(0)
{
}

StereoMatch::~StereoMatch(void)
{
}


int StereoMatch::init(int imgWidth, int imgHeight, const char* xmlFilePath)
{
    m_frameWidth = imgWidth;
    m_frameHeight = imgHeight;
    m_numberOfDisparies = 0;


    return loadCalibData(xmlFilePath);
}


int StereoMatch::loadCalibData(const char* xmlFilePath)
{
    try
    {
        cv::FileStorage fs(xmlFilePath, cv::FileStorage::READ);
        if (!fs.isOpened())
        {
            return (0);
        }

        cv::Size imageSize;
        cv::FileNodeIterator it = fs["imageSize"].begin();
        it >> imageSize.width >> imageSize.height;
        if (imageSize.width != m_frameWidth || imageSize.height != m_frameHeight)
        {
            return (-1);
        }

        vector<int> roiVal1;
        vector<int> roiVal2;

        fs["leftValidArea"] >> roiVal1;
        m_Calib_Roi_L.x = roiVal1[0];
        m_Calib_Roi_L.y = roiVal1[1];
        m_Calib_Roi_L.width = roiVal1[2];
        m_Calib_Roi_L.height = roiVal1[3];

        fs["rightValidArea"] >> roiVal2;
        m_Calib_Roi_R.x = roiVal2[0];
        m_Calib_Roi_R.y = roiVal2[1];
        m_Calib_Roi_R.width = roiVal2[2];
        m_Calib_Roi_R.height = roiVal2[3];


        //读取标定文件里映射矩阵

        fs["QMatrix"] >> m_Calib_Mat_Q;
        fs["remapX1"] >> m_Calib_Mat_Remap_X_L;
        fs["remapY1"] >> m_Calib_Mat_Remap_Y_L;
        fs["remapX2"] >> m_Calib_Mat_Remap_X_R;
        fs["remapY2"] >> m_Calib_Mat_Remap_Y_R;

        cv::Mat lfCamMat;
        fs["leftCameraMatrix"] >> lfCamMat;
        m_FL = lfCamMat.at<double>(0, 0);

        m_Calib_Mat_Mask_Roi = cv::Mat::zeros(m_frameHeight, m_frameWidth, CV_8UC1);
        cv::rectangle(m_Calib_Mat_Mask_Roi, m_Calib_Roi_L, cv::Scalar(255), -1);

        m_BM->setROI1(m_Calib_Roi_L);
        m_BM->setROI2(m_Calib_Roi_R);

        m_Calib_Data_Loaded = true;

        string method;
        fs["rectifyMethod"] >> method;
        if (method != "BOUGUET")
        {
            return (-2);
        }
        int y_min = (m_Calib_Roi_L.y > m_Calib_Roi_R.y) ? m_Calib_Roi_L.y : m_Calib_Roi_R.y;
        int x_min = (m_Calib_Roi_L.x > m_Calib_Roi_R.x) ? m_Calib_Roi_L.x : m_Calib_Roi_R.x;
        int y_max = (m_Calib_Roi_L.y + m_Calib_Roi_L.height < m_Calib_Roi_R.y + m_Calib_Roi_R.height) ? (m_Calib_Roi_L.y + m_Calib_Roi_L.height) : (m_Calib_Roi_R.y + m_Calib_Roi_R.height);
        int x_max = (m_Calib_Roi_L.x + m_Calib_Roi_L.width < m_Calib_Roi_R.x + m_Calib_Roi_R.width) ? (m_Calib_Roi_L.x + m_Calib_Roi_L.width) : (m_Calib_Roi_R.x + m_Calib_Roi_R.width);
        m_rect = cv::Rect(x_min, y_min, x_max - x_min, y_max - y_min);



    }
    catch (std::exception& e)
    {
        m_Calib_Data_Loaded = false;
        return (-99);
    }

    return 1;
}



int StereoMatch::bmMatch(cv::Mat& frameLeft, cv::Mat& frameRight, cv::Mat& disparity, cv::Mat& imageLeft, cv::Mat& imageRight,
                         cv::Mat &cropLeft, cv::Mat &cropRight)
{
    if (frameLeft.empty() || frameRight.empty())
    {
        disparity = cv::Scalar(0);
        return 0;
    }
    if (m_frameWidth == 0 || m_frameHeight == 0)
    {

        //calib_paras_zed_1280x720_1219_new
//        if (init(frameLeft.cols, frameLeft.rows, "../../calib/calib_paras_ZED_1280x720_201912261133.xml") == 0)
        if (init(frameLeft.cols, frameLeft.rows, "../../calib/calib_paras_zed_1280x720_1219_new.xml") == 0)
        {
            return 0;
        }

    }
    //利用映射矩阵m_Calib_Mat_Remap_X_L m_Calib_Mat_Remap_Y_L去左右两目相机的畸变，且极线平行

    if (m_Calib_Data_Loaded)
        remap(frameLeft, imageLeft, m_Calib_Mat_Remap_X_L, m_Calib_Mat_Remap_Y_L, cv::INTER_LINEAR);
    else
        frameLeft.copyTo(imageLeft);

    if (m_Calib_Data_Loaded)
        remap(frameRight, imageRight, m_Calib_Mat_Remap_X_R, m_Calib_Mat_Remap_Y_R, cv::INTER_LINEAR);
    else
        frameRight.copyTo(imageRight);
    //切出左右两目共视部分

    cropLeft = cv::Mat(imageLeft, m_rect);
    cropRight = cv::Mat(imageRight, m_rect);

    return 1;
}
//AWB
void AWB(cv::Mat &src, cv::Mat &dst)
{
    vector<Mat> imageRGB;
    cv::split(src, imageRGB);

    double R, G, B;
    B = cv::mean(imageRGB[0])[0];
    G = cv::mean(imageRGB[1])[0];
    R = cv::mean(imageRGB[2])[0];

    if (B != 0 && G != 0 && R != 0) {
        double KR, KG, KB;
        KB = (R + G + B) / (3 * B);
        KG = (R + G + B) / (3 * G);
        KR = (R + G + B) / (3 * R);

        imageRGB[0] = imageRGB[0] * KB;
        imageRGB[1] = imageRGB[1] * KG;
        imageRGB[2] = imageRGB[2] * KR;
    }

    cv::merge(imageRGB, dst);
}
void LoadTimestamps(const string &strPathToSequence, vector<double> &vTimestamps)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/video_time.txt";
    fTimes.open(strPathTimeFile.c_str());

    vTimestamps.reserve(5000);

    while (!fTimes.eof()) {
        string s;
        getline(fTimes, s);
        if (!s.empty())
        {
            string tmp = s.substr(0,s.find_first_of("\r"));

            double t;
            stringstream ss;
            ss<<s;
            ss >> t;

            vTimestamps.push_back(t/1e9);
        }
    }
}
bool flag_end;
int main()
{
    std::string pathToTimes = "/home/yunlei/Datasets/Homer02_zed_1280x720/";
    std::vector<double> vTimes;
    LoadTimestamps(pathToTimes, vTimes);
//    for(int i = 0; i<vTimes.size(); i++)
//    {
//        std::cout<<setprecision(14)<<vTimes[i]*1e9<<std::endl;
//    }
    std::vector<double> vTimesTrack;
    //ORB_SLAM2::System SLAM("../../Vocabulary/ORBvoc.txt","./zed_1249x603_20191226.yaml",ORB_SLAM2::System::STEREO,true);

    int cunt = 0;
    StereoMatch sm;
    VideoCapture capture_left;
    VideoCapture capture_right;
    Mat frame_left;
    Mat frame_right;

    capture_left.open("/home/yunlei/Datasets/Homer02_zed_1280x720/left.mp4");
    std::string output_path_left = "/home/yunlei/Datasets/Homer02_zed_1280x720/leftCrop//";

    capture_right.open("/home/yunlei/Datasets/Homer02_zed_1280x720/right.mp4");
    std::string output_path_right = "/home/yunlei/Datasets/Homer02_zed_1280x720/rightCrop/";

    Mat disparity;
    Mat rectifyImageL, rectifyImageR;
    Mat cropLeft, cropRight;

    if (!capture_left.isOpened()) {
        std::cout << "capture_left open failed!" << std::endl;
    }
    if (!capture_right.isOpened()) {
        std::cout << "capture_right open failed!" << std::endl;
    }

    while (capture_left.isOpened() && capture_right.isOpened())
    {
        capture_left >> frame_left;
        if (frame_left.empty())
        {
            cout << "frame right is empty! cunt = " << cunt << std::endl;
            break;//continue;
        }
        capture_right >> frame_right;
        if (frame_right.empty())
        {
            flag_end = false;
            cout << "frame right is empty! cunt = " << cunt << std::endl;
            break;//continue;
        }
        std::stringstream ss;
        ss<<cunt;
        string frameName;
        ss>>frameName;

        string leftFramePath = output_path_left + frameName + ".png";
        string rightFramePath = output_path_right + frameName + ".png";

        sm.bmMatch(frame_left, frame_right, disparity, rectifyImageL, rectifyImageR, cropLeft, cropRight);
        AWB(cropLeft, cropLeft);
        AWB(cropRight, cropRight);




       cv::imshow("leftOri", frame_left);
     　cv::waitKey(10);
       cv::imshow("rightOri", frame_right);
    　　cv::waitKey(10);
    　　cv::imshow("leftCrop", cropLeft);
    　　cv::waitKey(10);
    　　cv::imshow("rightCrop", cropRight);
    　　cv::waitKey(10);
    
        ///Just for test
        // joint left and right image into one image.
//将两幅进行去畸变的图像拼接到一起，并画上横线，可以直观的查看，去畸变的效果
//先列，后行
        int imgWidth = cropLeft.cols;
        int imgHeight = cropLeft.rows;
        cv::Mat testImg(imgHeight,2*imgWidth+1, cropLeft.type());

        cropLeft.copyTo(testImg(cv::Rect(0,0, imgWidth,imgHeight)));
        cropRight.copyTo(testImg(cv::Rect(imgWidth+1,0,imgWidth, imgHeight)));
        //std::cout<<"testImg size: "<<testImg.cols<<"x"<<testImg.rows<<std::endl;

        cv::Point startPoint = cv::Point(0,0);
        cv::Point endPoint = cv::Point(2*imgWidth+1, 50);
        int tmpHeight = 0;
        for(int i = 0; i<imgHeight/30; i++)
        {

            startPoint = cv::Point(0,tmpHeight);
            endPoint = cv::Point(2*imgWidth+1, tmpHeight);
            cv::line(testImg, startPoint, endPoint, cv::Scalar(0,255,0));
            tmpHeight+=30;
        }


        if(cunt == 50)
        {
            cv::imshow("testImg", testImg);
            cv::waitKey(0);
        }
        cunt++;
      cv::imwrite(leftFramePath, cropLeft);
      cv::imwrite(rightFramePath, cropRight);

    }
     cout << "the video is end! " << "-----------------" << endl;

    return 0;
}