双目测距(四)

作者: Parker2019 | 来源:发表于2020-02-12 14:07 被阅读0次

Code:

/******************************/
/*        立体匹配和测距        */
/******************************/

#include <opencv2/opencv.hpp>  
#include <iostream>  

using namespace std;
using namespace cv;

const int imageWidth = 640;                             //摄像头的分辨率  
const int imageHeight = 480;
Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后,会对图像进行裁剪,这里的validROI就是指裁剪之后的区域  
Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表  
Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R,投影矩阵P 重投影矩阵Q
Mat xyz;              //三维坐标

Point origin;         //鼠标按下的起始点
Rect selection;      //定义矩形选框
bool selectObject = false;    //是否选择对象

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

/*
事先标定好的相机的参数
fx 0 cx
0 fy cy
0 0  1
*/
//标定数据来自Matlab
Mat cameraMatrixL = (Mat_<double>(3, 3) << 452.8131, 0, 303.0163,
    0, 452.8235, 228.0770,
    0, 0, 1);
Mat distCoeffL = (Mat_<double>(4, 1) << -0.0241,0.4826,0,0);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 453.6927, 0, 311.4093,
    0, 453.9168, 237.1017,
    0, 0, 1);
Mat distCoeffR = (Mat_<double>(4, 1) << 0.0171,-0.0338,0,0);

Mat T = (Mat_<double>(3, 1) << -70.2056, 0.1296, 0.6029);//T平移向量
Mat rec = (Mat_<double>(3, 1) << 0.0076, -0.0140, 0.00240);//rec旋转向量
Mat R;//R 旋转矩阵


/*****立体匹配*****/
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(numDisparities*16+16);//视差窗口,即最大视差值与最小视差值之差,窗口大小必须是16的整数倍,int型
    bm->setTextureThreshold(10); 
    bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp, disp8;
    bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图
    disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 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 (selectObject)
    {
        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);
        selectObject = true;
        cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
        break;
    case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
        selectObject = false;
        if (selection.width > 0 && selection.height > 0)
        break;
    }
}


/*****主函数*****/
int main()
{
    /*
    立体校正
    */
    Rodrigues(rec, R); //Rodrigues变换
    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
        0, imageSize, &validROIL, &validROIR);
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

    /*
    读取图片
    */
    rgbImageL = imread("C:\\Users\\INTEL\\Desktop\\test_workspace\\dual_cam\\calibration\\src\\photos\\left0.jpg", CV_LOAD_IMAGE_COLOR);
    cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
    rgbImageR = imread("C:\\Users\\INTEL\\Desktop\\test_workspace\\dual_cam\\calibration\\src\\photos\\right0.jpg", CV_LOAD_IMAGE_COLOR);
    cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);

    imshow("ImageL Before Rectify", grayImageL);
    imshow("ImageR Before Rectify", grayImageR);

    /*
    经过remap之后,左右相机的图像已经共面并且行对准了
    */
    remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
    remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

    /*
    把校正结果显示出来
    */
    Mat rgbRectifyImageL, rgbRectifyImageR;
    cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR);  //伪彩色图
    cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);

    //单独显示
    //rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
    //rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
    imshow("ImageL After Rectify", rgbRectifyImageL);
    imshow("ImageR After Rectify", rgbRectifyImageR);

    //显示在同一张图上
    Mat canvas;
    double sf;
    int w, h;
    sf = 600. / MAX(imageSize.width, imageSize.height);
    w = cvRound(imageSize.width * sf);
    h = cvRound(imageSize.height * sf);
    canvas.create(h, w * 2, CV_8UC3);   //注意通道

    //左图像画到画布上
    Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分  
    resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小  
    Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),                //获得被截取的区域    
        cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
    //rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形  
    cout << "Painted ImageL" << endl;

    //右图像画到画布上
    canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分  
    resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
    Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
        cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
    //rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
    cout << "Painted ImageR" << endl;

    //画上对应的线条
    for (int i = 0; i < canvas.rows; i += 16)
        line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
    imshow("rectified", canvas);

    /*
    立体匹配
    */
    namedWindow("disparity", CV_WINDOW_AUTOSIZE);
    // 创建SAD窗口 Trackbar
    createTrackbar("BlockSize:\n", "disparity",&blockSize, 8, stereo_match);
    // 创建视差唯一性百分比窗口 Trackbar
    createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
    // 创建视差窗口 Trackbar
    createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
    //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数,一般取0)
    setMouseCallback("disparity", onMouse, 0);
    stereo_match(0,0);

    waitKey(0);
    return 0;
}

这个矩阵很重要
/*
事先标定好的相机的参数
fx 0 cx
0 fy cy
0 0 1
*/


世界坐标系得到深度

最后得到的深度大概在520mm左右,与贴在墙上的纸距离差不多。

相关文章

  • 双目测距(四)

    Code: 这个矩阵很重要/*事先标定好的相机的参数fx 0 cx0 fy cy0 0 1*/ 最后得到的深度大...

  • 双目测距

    左边中轴到点P在左相机的成像点也就是xl的距离为 xl 右边中轴到点P在右相机的成像点也就是xr的距离为 ...

  • 双目标定(三)

    借助Matlab标定 Matlab标定结果 向OpenCV中写入标定结果并测试。参见双目测距(四)

  • OpenCV实现双目测距

    原理图示 原理很简单,利用了相似三角形计算距离,所以双目测距的主要任务在于前期摄像头的定标、双目图像点的特征匹配上...

  • Robomasters视觉研究全记录

    创建于20171129 12/08 双目测距 https://www.cnblogs.com/daihengche...

  • 双目视觉棋盘标定板的选择

    网上有的就不说了,这里说些细节 1.双目测距的注意事项: 在测距之前一定要确定自己测量的目标的大概距离(比如1.5...

  • opencv3.2 单目摄像头的标定与矫正

    最近打省电赛,与双目立体视觉相关。要实现双目测距首先要进行摄像头的标定,单目标定主要是为了测定摄像头的内参矩阵和畸...

  • 机器视觉学习笔记: 一个双目测距的简单实例

    1.实例目标 学习OpenCV也一月有余了,遂想进行一个双目测距的简单实验,先解决从无到有,再解决锦上添花。 该实...

  • 人工智能课第三次展示

    AI Car 第三次展示报告 实现的功能 基于摄像头的双目测距 基于卷积神经网络的图像识别 基于长短期记忆网络的音...

  • 2018-10-09

    项目四:超声波测距原理 首先先来了解下超声波测距的原理: 人们可以听到的声音的频率为20Hz~2KHz,也就是可...

网友评论

    本文标题:双目测距(四)

    本文链接:https://www.haomeiwen.com/subject/kmpdzctx.html