对极几何相关程序

#include <iostream>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <fstream>
#include <opencv2/core/core.hpp>
#define PATH "/Users/mac/ClionProjects/opencv_test/";
using namespace std;//标准库　命名空间
using namespace cv; //opencv库命名空间

vector<Point2f> points1, points2, points3;
String path = "/Users/mac/ClionProjects/opencv_test/";

// 双目校正用到量, 图像分辨率为2040*2048
double scale = 0.2;
const int imageWidth=2040, imageHeight = 2048;
Size imageSize(imageWidth, imageHeight);
Mat rectifyImgL,rectifyImgR;
Rect validRoiL, validRoiR; // 裁剪后的区域
Mat mapLx, mapLy, mapRx, mapRy; //映射
Mat Rl, Rr, Pl, Pr, Q; // 校正旋转矩阵R 投影矩阵P 重投影矩阵Q
Mat xyz;

Point origin; // 鼠标起始点
Rect selection; // 框选
bool selectOb = false; // 是否选择对象

int blockSize = 0, uniqueRatio = 0, numDisparity = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9);

//使用matlab标定好的数据,使用重载后的流操作符初始化
Mat camKKL = (Mat_<double >(3, 3)<<2194.63, 0, 992.88, 0, 2196.167, 1043.53, 0, 0, 1);
Mat camKKR = camKKL;
Mat distCoeL = (Mat_<double>(1, 4)<<-0.04, 0.13485, -0.0017, -0.0008);
Mat distCoeR = distCoeL;

void stereo_match(int , void*);
static void onMouse(int event, int x, int y, int, void*);// 鼠标操作回调
void readCsv(string filename, vector<Point2f>& points);//初始化，从csv中读取数据,读取后结果存到对应的points中
void writeXML(Mat mat, string name); // 数据写入xml

// 计算本质矩阵， 并进行双目校正 ，产生校正变换，将校正后的图像放在rectifyImg中
void myCalib(InputArray points1, Mat camKKL, Mat distCoeL, InputArray points2, Mat camKKR, Mat distCoeR, string pic1, string pic2);

int main()
{
    string filename1 = "point1B.csv";
    string filename2 = "point2M.csv";
    readCsv(filename1, points1);
    readCsv(filename2, points2);
    myCalib(points1, camKKL, distCoeL, points2, camKKR, distCoeR, "B.jpg", "M.jpg");

    return 0;

}


void myCalib(InputArray points1, Mat camKKL, Mat distCoeL, InputArray points2, Mat camKKR, Mat distCoeR, string pic1, string pic2){
    Mat EssenMatrix, R, T;
    EssenMatrix= findEssentialMat(points1, points2, camKKL);

    recoverPose(EssenMatrix, points1, points2, camKKL, R, T);
    stereoRectify(camKKL, distCoeL, camKKR, distCoeR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, 1, imageSize, &validRoiL, &validRoiR);
    fisheye::initUndistortRectifyMap(camKKL, distCoeL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
    fisheye::initUndistortRectifyMap(camKKR, distCoeR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
    cout<<Pr<<endl<<Q;

    Mat matPic1 = imread(path+pic1);
    Mat matPic2 = imread(path+pic2);
    remap(matPic1, rectifyImgL, mapLx, mapLy, INTER_LINEAR);
    remap(matPic2, rectifyImgR, mapRx, mapRy, INTER_LINEAR);

    //太大不能显示则放缩
    Mat SRectifyPic1, SRectifyPic2;
    resize(rectifyImgL, SRectifyPic1, Size(), scale, scale, INTER_LINEAR);
    resize(rectifyImgR, SRectifyPic2, Size(), scale, scale, INTER_LINEAR);
    imshow("imageL", SRectifyPic1);
    imshow("imageR", SRectifyPic2);
    cvStereo
    waitKey(0);


}

void writeXML(Mat mat, string name){
    FileStorage fs(path+name, FileStorage::WRITE);
    fs<<"Mat"<<mat;
    fs.release();
}

void readCsv(string filename, vector<Point2f>& points){
    ifstream fin(path+filename);
    string line, value;
    int xflag=0, index=0;
    float xdata, ydata;
    while(fin.good()){
        getline(fin, value);
        //cout<<value<<endl;
        int delim = value.find(",");
        //cout<<value.substr(0, delim)<<"  "<<value.substr(delim+1, value.length()-delim-2)<<endl;
        xdata = stof(value.substr(0, delim));
        ydata = stof(value.substr(delim+1, value.length()-delim-2));
        points.push_back(Point2f(xdata, ydata));

    }

}

void stereo_match(int,void*)
{
    bm->setBlockSize(2*blockSize+5);     //SAD窗口大小，5~21之间为宜
    bm->setROI1(validRoiL);
    bm->setROI2(validRoiR);
    bm->setPreFilterCap(31);
    bm->setMinDisparity(0);  //最小视差，默认值为0, 可以是负值，int型
    bm->setNumDisparities(numDisparity*16+16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(uniqueRatio);//uniquenessRatio主要可以防止误匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp, disp8;
    bm->compute(rectifyImgL, rectifyImgR, disp);//输入图像必须为灰度图
    disp.convertTo(disp8, CV_8U, 255 / ((numDisparity * 16 + 16)*16.));//计算出的视差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
    xyz = xyz * 16;
    imshow("disparity", disp8);
}


static void onMouse(int event, int x, int y, int, void*)
{
    if (selectOb)
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
    }

    switch (event)
    {
        case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件
            origin = Point(x, y);
            selection = Rect(x, y, 0, 0);
            selectOb = true;
            cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
            break;
        case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
            selectOb = false;
            if (selection.width > 0 && selection.height > 0)
                break;
    }
}