【opencv學習】完全基於opencv的雙目景深與測距的實現

阿新 • • 發佈：2018-12-16

1 說明

執行環境： 1.windows10 2.opencv 2.4.9 3.visual studio 2013 4.兩顆微軟HD-3000攝像頭

2 雙目測距原理

先說一下自己對雙目視覺實現原理的理解，不保證都是正確的：首先就是這個經典的圖：這個圖簡單的說明了雙目測距的基本原理，就是想方設法求出距離Z。右下角Z的那個等式右邊的引數中： f是每個攝像頭自己的焦距，也就是感測器到鏡頭之間的距離。 T是兩個攝像頭的鏡頭之間的距離，這些都是確定的。 d是不確定的，d是一個物體在分別兩個感測器上所成的像，也就是xl和xr之間的距離，是個變數。

所以為了得出距離，每次就需要獲得d的值，之後根據相似三角形原理就可以求出Z。

現在問題來了，為什麼上面那個圖，攝像頭的感測器和鏡頭位置是反的呢？看這個圖：小孔成像原理為了數學上的處理方便，研究人員通常習慣採用虛擬平面V來替代成像平面I，其中虛擬平面V位於焦距平面F與物體之間，與成像平面關於焦點平面對稱。

3 opencv實現雙目測距的原理

上面就是雙目視覺的簡單原理介紹，公式什麼的不想往上寫了，跟實現什麼的也沒多大關係。原理歸原理，用opencv實現雙目視覺的時候就是又一碼事了。在opencv上實現雙目測距主要步驟是：

1.雙目校正和標定，獲得攝像頭的引數矩陣：進行標定得出倆攝像頭的引數矩陣 cvStereoRectify 執行雙目校正 initUndistortRectifyMap 分別生成兩個影象校正所需的畫素對映矩陣 cvremap 分別對兩個影象進行校正

2.立體匹配，獲得視差圖： stereoBM生成視差圖預處理：影象歸一化，減少亮度差別，增強紋理匹配過程：滑動sad視窗，沿著水平線進行匹配搜尋，由於校正後左右圖片平行，左圖的特徵可以在右圖對應行找到最佳匹配再過濾：去除壞的匹配點通過uniquenessratio 輸出視差圖disparity：如果左右匹配點比較稠密，匹配點多，得到的影象與原圖相似度比較大，如果匹配點比較稀疏，得到的點與原圖相似度比較小

3.得出測距：

把生成的視差圖輸入 reprojectImageTo3D（）函式，生成3D點雲，3D點雲中儲存有2D影象的三維座標，再讀出每幀影象的三維座標中的z軸的值，就得出了距離資料。

4 雙目測距程式碼說明

我的這個雙目程式寫是很簡單和普通，部分關鍵程式碼借鑑了大神的程式碼，其實說是程式碼其實只不過是某些opencv函式的使用方式而已，在大體上還是有很大的不同：

首先大神的程式整個都是基於MFC的，這樣做的話如果你想移植到arm板子上或是linux系統上就會很麻煩，所以我把整個程式包括影象顯示，視差圖顯示，距離顯示等都完全使用opencv的函式來實現。

再者，在大神的程式中輸出距離的方式，我的理解是，首先得檢測到最近的物體的輪廓，然後在三維點雲中提取出這個輪廓的距離座標。但是實現起來不是很理想，如果視差圖的質量不高，根本檢測不到輪廓，不會觸發這個函式，更別提距離了。所以在我的程式中，得出距離的方式是用滑鼠點視差圖的某個點，就會顯示這個點的距離，不過至今距離資訊也不是很準確，有可能是一些引數沒弄好。

需要說明的是，對於兩個攝像頭來說，他們之間引數矩陣只有一個（如果兩攝像頭間相對位置不變的話），所以定標過程只需要一次即可。所以我的程式並沒有弄標定的東西（嫌麻煩），而是從外部讀取calib_paras.xml這個引數檔案，這個檔案可以通過大神的程式碼來定標然後生成出來，還應該可以從matlab的標定工具箱來生成（下面的連結），不過我沒弄（matlab生成出的引數資料不知道怎麼用）。 http://www.vision.caltech.edu/bouguetj/calib_doc/

5 雙目測距的程式碼和實現

我的這個程式只負責開啟攝像頭，顯示影象，生成視差圖，顯示視差圖，求出點雲，得出距離，顯示距離。也就是說他是在標定過程之後開始的，程式裡沒有攝像頭定標的過程和函式，所以要正確執行是需要calib_paras.xml這個檔案的，也就是標定後得出的引數檔案，可以通過執行大神的程式碼定標後生成。而且為了保證你能正確成功的執行，最好確保你的電腦能執行的了大神的程式（地址如下）。 https://github.com/yuhuazou/StereoVision

我的程式：

#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
//#include <cv.h>
#include <cxmisc.h>
#include <highgui.h>
#include <cvaux.h>
#include <iostream>
#include <ctype.h>
//#include <unistd.h>
#include <stdlib.h>

#include <vector>
#include <string>
#include <algorithm>
#include <ctype.h>
#include <stdarg.h>
#include <string.h>

#include <stdio.h>


using namespace cv;
using namespace std;

//vector<Point2f> point1, point2;
bool left_mouse = false;
Point2f point;
int pic_info[4];
Mat gray, prevGray, image, image1;
const Scalar GREEN = Scalar(0, 255, 0);
int rect_width = 0, rect_height = 0;
Point tmpPoint;
int num = 0;
int m_frameWidth = 640;
int m_frameHeight = 480;
bool    m_Calib_Data_Loaded;        // 是否成功載入定標引數
cv::Mat m_Calib_Mat_Q;              // Q 矩陣
cv::Mat m_Calib_Mat_Remap_X_L;      // 左檢視畸變校正畫素座標對映矩陣 X
cv::Mat m_Calib_Mat_Remap_Y_L;      // 左檢視畸變校正畫素座標對映矩陣 Y
cv::Mat m_Calib_Mat_Remap_X_R;      // 右檢視畸變校正畫素座標對映矩陣 X
cv::Mat m_Calib_Mat_Remap_Y_R;      // 右檢視畸變校正畫素座標對映矩陣 Y
cv::Mat m_Calib_Mat_Mask_Roi;       // 左檢視校正後的有效區域
cv::Rect m_Calib_Roi_L;             // 左檢視校正後的有效區域矩形
cv::Rect m_Calib_Roi_R;             // 右檢視校正後的有效區域矩形
double          m_FL;
//CvStereoBMState *BMState = cvCreateStereoBMState();
int m_numberOfDisparies;            // 視差變化範圍
cv::StereoBM    m_BM;
CvMat* vdisp = cvCreateMat(480, 640, CV_8U);
cv::Mat img1, img2, img1p, img2p, disp, disp8u, pointClouds, imageLeft, imageRight, disparityImage, imaget1;
static IplImage *framet1 = NULL;
static IplImage *framet2 = NULL;
static IplImage *framet3 = NULL;
static IplImage *framet = NULL;


static void onMouse(int event, int x, int y, int /*flags*/, void* /*param*/){


    Mat mouse_show;
    image.copyTo(mouse_show);

    //char buffer[100];
    //sprintf(buffer, "D:\\l%d.jpg", num);
    //string t1(buffer);

    //sprintf(buffer, "D:\\r%d.jpg", num);
    //string t(buffer);



    if (event == CV_EVENT_LBUTTONDOWN)
    {
        pic_info[0] = x;
        pic_info[1] = y;
        cout << "x:" << pic_info[0] << "y:" << pic_info[1] << endl;
        left_mouse = true;

        //用於儲存列印圖片
        //imwrite(t, image);
    //  imwrite(t1, image1);
    //  num = num++;

    }
    else if (event == CV_EVENT_LBUTTONUP)
    {
        left_mouse = false;
    }
    else if ((event == CV_EVENT_MOUSEMOVE) && (left_mouse == true))
    {
    }
}



int loadCalibData()
{
    // 讀入攝像頭定標引數 Q roi1 roi2 mapx1 mapy1 mapx2 mapy2
    try
    {
        cv::FileStorage fs("calib_paras.xml", cv::FileStorage::READ);
        cout << fs.isOpened() << endl;

        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_Q.at<double>(3, 2) = -m_Calib_Mat_Q.at<double>(3, 2);

        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.state->roi1 = m_Calib_Roi_L;
        m_BM.state->roi2 = m_Calib_Roi_R;

        m_Calib_Data_Loaded = true;

        string method;
        fs["rectifyMethod"] >> method;
        if (method != "BOUGUET")
        {
            return (-2);
        }

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

    return 1;


}
void updatebm()
{
    m_BM.state->preFilterCap = 31;
    m_BM.state->SADWindowSize = 19;
    m_BM.state->minDisparity = 0;
    m_BM.state->numberOfDisparities = 96;
    m_BM.state->textureThreshold = 10;
    m_BM.state->uniquenessRatio = 25;
    m_BM.state->speckleWindowSize = 100;
    m_BM.state->speckleRange = 32;

    m_BM.state->disp12MaxDiff = -1;

}
int  bmMatch(cv::Mat& frameLeft, cv::Mat& frameRight, cv::Mat& disparity, cv::Mat& imageLeft, cv::Mat& imageRight)
{

    // 輸入檢查
    if (frameLeft.empty() || frameRight.empty())
    {
        disparity = cv::Scalar(0);
        return 0;
    }
    if (m_frameWidth == 0 || m_frameHeight == 0)
    {
        //if (init(frameLeft.cols, frameLeft.rows, "calib_paras.xml"/*待改為由本地設定檔案確定*/) == 0) //執行類初始化
        //  {
        return 0;
        //}
    }

    // 轉換為灰度圖
    cv::Mat img1proc, img2proc;
    cvtColor(frameLeft, img1proc, CV_BGR2GRAY);
    cvtColor(frameRight, img2proc, CV_BGR2GRAY);

    // 校正影象，使左右檢視行對齊    
    cv::Mat img1remap, img2remap;
    //cout << m_Calib_Data_Loaded << endl;

    if (m_Calib_Data_Loaded)
    {
        remap(img1proc, img1remap, m_Calib_Mat_Remap_X_L, m_Calib_Mat_Remap_Y_L, cv::INTER_LINEAR);     // 對用於視差計算的畫面進行校正
        remap(img2proc, img2remap, m_Calib_Mat_Remap_X_R, m_Calib_Mat_Remap_Y_R, cv::INTER_LINEAR);
    }
    else
    {
        img1remap = img1proc;
        img2remap = img2proc;
    }

    // 對左右檢視的左邊進行邊界延拓，以獲取與原始檢視相同大小的有效視差區域
    cv::Mat img1border, img2border;
    if (m_numberOfDisparies != m_BM.state->numberOfDisparities)
        m_numberOfDisparies = m_BM.state->numberOfDisparities;
    copyMakeBorder(img1remap, img1border, 0, 0, m_BM.state->numberOfDisparities, 0, IPL_BORDER_REPLICATE);
    copyMakeBorder(img2remap, img2border, 0, 0, m_BM.state->numberOfDisparities, 0, IPL_BORDER_REPLICATE);

    // 計算視差
    cv::Mat dispBorder;


    m_BM(img1border, img2border, dispBorder);
    //cvFindStereoCorrespondenceBM(img1border, img2border, dispBorder,BMState);
    // 擷取與原始畫面對應的視差區域（捨去加寬的部分）
    cv::Mat disp;
    disp = dispBorder.colRange(m_BM.state->numberOfDisparities, img1border.cols);
    disp.copyTo(disparity, m_Calib_Mat_Mask_Roi);

    //reprojectImageTo3D(dispBorder, pointClouds, m_Calib_Mat_Q, true);

    // 輸出處理後的影象
    //cout << m_Calib_Data_Loaded << endl;
    if (m_Calib_Data_Loaded)
    {
        remap(frameLeft, imageLeft, m_Calib_Mat_Remap_X_L, m_Calib_Mat_Remap_Y_L, cv::INTER_LINEAR);
        rectangle(imageLeft, m_Calib_Roi_L, CV_RGB(0, 0, 255), 3);
    }

    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);
    rectangle(imageRight, m_Calib_Roi_R, CV_RGB(0, 0, 255), 3);


    return 1;
}

int getDisparityImage(cv::Mat& disparity, cv::Mat& disparityImage, bool isColor)
{
    // 將原始視差資料的位深轉換為 8 位
    cv::Mat disp8u;
    if (disparity.depth() != CV_8U)
    {
        if (disparity.depth() == CV_8S)
        {
            disparity.convertTo(disp8u, CV_8U);
        }
        else
        {
            disparity.convertTo(disp8u, CV_8U, 255 / (m_numberOfDisparies*16.));
        }
    }
    else
    {
        disp8u = disparity;
    }

    // 轉換為偽彩色影象 或 灰度影象
    if (isColor)
    {
        if (disparityImage.empty() || disparityImage.type() != CV_8UC3 || disparityImage.size() != disparity.size())
        {
            disparityImage = cv::Mat::zeros(disparity.rows, disparity.cols, CV_8UC3);
        }

        for (int y = 0; y<disparity.rows; y++)
        {
            for (int x = 0; x<disparity.cols; x++)
            {
                uchar val = disp8u.at<uchar>(y, x);
                uchar r, g, b;

                if (val == 0)
                    r = g = b = 0;
                else
                {
                    r = 255 - val;
                    g = val < 128 ? val * 2 : (uchar)((255 - val) * 2);
                    b = val;
                }

                disparityImage.at<cv::Vec3b>(y, x) = cv::Vec3b(r, g, b);

            }
        }
    }
    else
    {
        disp8u.copyTo(disparityImage);
    }

    return 1;
}

int getPointClouds(cv::Mat& disparity, cv::Mat& pointClouds)
{
    if (disparity.empty())
    {
        return 0;
    }

    //計算生成三維點雲
//  cv::reprojectImageTo3D(disparity, pointClouds, m_Calib_Mat_Q, true);

    reprojectImageTo3D(disparity, pointClouds, m_Calib_Mat_Q, true);

    pointClouds *= 1.6;


    for (int y = 0; y < pointClouds.rows; ++y)
    {
        for (int x = 0; x < pointClouds.cols; ++x)
        {
            cv::Point3f point = pointClouds.at<cv::Point3f>(y, x);
            point.y = -point.y;
            pointClouds.at<cv::Point3f>(y, x) = point;
        }
    }

    return 1;
}

void detectDistance(cv::Mat& pointCloud)
{
    if (pointCloud.empty())
    {
        return;
    }

    // 提取深度影象
    vector<cv::Mat> xyzSet;
    split(pointCloud, xyzSet);
    cv::Mat depth;
    xyzSet[2].copyTo(depth);

    // 根據深度閾值進行二值化處理
    double maxVal = 0, minVal = 0;
    cv::Mat depthThresh = cv::Mat::zeros(depth.rows, depth.cols, CV_8UC1);
    cv::minMaxLoc(depth, &minVal, &maxVal);
    double thrVal = minVal * 1.5;
    threshold(depth, depthThresh, thrVal, 255, CV_THRESH_BINARY_INV);
    depthThresh.convertTo(depthThresh, CV_8UC1);
    //imageDenoising(depthThresh, 3);

    double  distance = depth.at<float>(pic_info[0], pic_info[1]);
    cout << "distance:" << distance << endl;
}


int main(int argc, char** argv)
{
    //讀取攝像頭
    VideoCapture cap(0); 
    VideoCapture cap1(1);

    if (!cap.isOpened())
    {
        cout << "error happened while open cam 1"<<endl;
        return -1;
    }
    if (!cap1.isOpened())
    {
        cout << "error happened while open cam 2" << endl;
        return -1;
    }

    namedWindow("left", 1);
    namedWindow("right", 1);
    namedWindow("disparitycolor", 1);

    setMouseCallback("disparitycolor", onMouse, 0);


    loadCalibData();

    cout << m_Calib_Data_Loaded << endl;

    while (true)
    {

            Mat frame;
            Mat frame1;
            cap.read(frame);
            cap1.read(frame1);
            if (frame.empty())          break;
            if (frame1.empty())         break;

            frame.copyTo(image);
            frame1.copyTo(image1);
            updatebm();
            bmMatch(frame, frame1, disp, imageLeft, imageRight);
            imshow("left", imageLeft);
            imshow("right", imageRight);

            getDisparityImage(disp, disparityImage, true);
            //  framet2 = &IplImage(disparityImage);
            //  cvShowImage("disparitycolor", framet2);

                getPointClouds(disp, pointClouds);

            imshow("disparitycolor", disparityImage);

            detectDistance(pointClouds);

            waitKey(100);


    }
    //std::swap(point2, point1);
    //  cv::swap(prevGray, gray);

    return 0;

}

我的使用的雙目攝像頭這裡寫圖片描述正面照片程式跑出來的效果截圖截圖中分別是左右攝像頭的影象，距離顯示，和視差圖顯示。

能看出視差圖不是很準，首先是因為引數沒時間仔細調，再關鍵是總是有人來動我的攝像頭，花了半天時間來標定好，別人拿手一掰一碰，倆攝像頭相對位置就又變了，前面標定的工作全白費，後來索性就不管了。

視差圖不準，距離資訊也肯定不是很準，大多數情況是16000，上面的圖裡用滑鼠點視差圖中紅色那一條，距離顯示中首先會輸出該點的x，y座標，然後就是該點的距離座標，也就是這個點z軸的值。

總體來說就是還得調。

6 接下來

以上就差不多是前段時間弄的東西，本打算把雙目測距移植到NVIDIA jetson tk1上看看效果，不過沒弄完，目前程序是：上面的程式碼已經可以在tk1上編譯成功了，就是死活執行不了，我懷疑是tk那個板子是不是不支援開啟和顯示倆攝像頭，這個問題花點時間我覺得應該是可以解決的，不過現在弄別的東西了，也懶得查了。

接下來就是要弄別的東西了，opencv的東西估計得放一放了。

【opencv學習】完全基於opencv的雙目景深與測距的實現

目錄

1 說明

2 雙目測距原理

3 opencv實現雙目測距的原理

4 雙目測距程式碼說明

5 雙目測距的程式碼和實現

6 接下來

【opencv學習】完全基於opencv的雙目景深與測距的實現

【機器學習】最簡單易懂的行人檢測功能實現

【機器學習】決策樹演算法（二）— 程式碼實現

王小草【機器學習】筆記--主題模型LDA實踐與應用

【BS學習】精通CSS.DIV網頁樣式與佈局總結

【機器學習】樸素貝葉斯基本介紹+程式碼實現

【深度學習】3：BP神經網路與MNIST資料集實現手寫數字識別

王小草【深度學習】筆記第七彈--RNN與應用案例：注意力模型與機器翻譯

【Oracle學習】之事務（Transaction）與鎖

【ES6學習】— (1)ES6簡介、let與const命令以及變數的解構賦值

【分散式架構】(10)---基於Redis元件的特性，實現一個分散式限流

【機器學習】最容易實現的基於OpenCV的人臉檢測程式碼、檢測器及檢測效果

【OpenCV學習】縮放影象

【OpenCV學習】Canny邊緣檢測

【機器學習】C++與OpenCV、Tensorflow-python聯合呼叫

【OpenCV學習】【一】關於影象疊加以及原理解釋（結合MATLAB）

【深度學習】Ubuntu16.04+tensorflow+opencv+pygame 執行FlappyBird（畫素小鳥）程式碼（4）

【opencv學習】矩陣CvMat的兩種宣告和初始化方法

【機器學習】DBSCAN Algorithms基於密度的聚類算法

【PHP學習】—apache配置虛擬主機（基於域名）

【opencv學習】完全基於opencv的雙目景深與測距的實現

目錄

1 說明

2 雙目測距原理

3 opencv實現雙目測距的原理

4 雙目測距程式碼說明

5 雙目測距的程式碼和實現

6 接下來

相關推薦