相機模型和雙目立體匹配完成一個基於KITTI立體相機採集圖片的立體圖像匹配程序，生成視差圖像和3D點雲圖像

一、針孔相機模型

其中P點是三維空間中的一點，P’點是P在圖片上的投影點，O是相機座標系的原點，O’是物理成像平面的原點。可得Z/f=X/X’=Y/Y’，即：X’=fX/Z，Y’=fY/Z，其中[X,Y,Z]是P點在相機座標系的座標。

二、雙目相機模型

雙目相機一般由水平放置的左眼相機和右眼相機組成，可以把兩個相機都看作針孔相機。因爲是水平放置的，意味着兩個相機的光圈中心都位於x軸上，兩者之間的距離稱爲雙目相機的基線(Baseline)。雙目相機的成像模型如下：

三、OpenCV實例實現用Pangolin生成視差圖像和3D點雲圖像

1.新建一個文件夾（我的名字是test）

2.在test文件夾下新建一個空的CMakeLists.txt文件

3.在test文件夾下打開新的終端，編譯

cmake後會生成很多編譯的中間文件以及makefile文件，所以一般建議新建一個新的目錄，目錄名爲build.

make根據生成makefile文件，編譯程序。

mkdir build
cd build
cmake ..
make

4.在test目錄下創建一個main.cpp文件

注意：代碼中圖片的存放位置要放在build目錄下

#include <iostream>
#include <chrono>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Core>
#include<opencv2/calib3d/calib3d.hpp>
#include <pangolin/pangolin.h>
using namespace std;
using namespace Eigen;
using namespace cv;

//高博用pangolin中顯示點雲
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);
int main ( int argc, char** argv )
{
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;// 內參
    double b = 0.573;// 基線
        
    cout << "OpenCV version : " << CV_VERSION << endl;
   
    Mat leftImg=imread("left.png",0);
    Mat rightImg=imread("right.png",0);
  
    imshow ( "leftImg", leftImg);
    imshow ( "rightImg", rightImg);
    waitKey ( 0 );
    
    //OpenCV實現的SGBM立體匹配算法
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(
        0,//minDisparity 最小視差
        96, //numDisparities 視差搜索範圍，值必需爲16的整數倍。最大搜索邊界=最小視差+視差搜索範圍
        9, //blockSize 塊大小
        //8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
        8 * 9 * 9, //P1 控制視差變化平滑性的參數。P1、P2的值越大，視差越平滑。P1是相鄰像素點視差增/減 1 時的懲罰係數；P2是相鄰像素點視差變化值大於1時的懲罰係數。P2必須大於P1。
        //32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize
        32 * 9 * 9, //P2
        1, //disp12MaxDiff 左右一致性檢測最大容許誤差閾值。
        63, //preFilterCap,預處理時映射參數
        10, //uniquenessRatio 唯一性檢測參數，
        100, //speckleWindowSize 視差連通區域像素點個數的大小。對於每一個視差點，當其連通區域的像素點個數小於speckleWindowSize時，認爲該視差值無效，是噪點。
        32//speckleRange 視差連通條件，在計算一個視差點的連通區域時，當下一個像素點視差變化絕對值大於speckleRange就認爲下一個視差像素點和當前視差像素點是不連通的。
    );
    
    Mat disparity_sgbm, disparity;
    sgbm->compute(leftImg, rightImg, disparity_sgbm); //計算視差圖
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);//得到視差圖
    
    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    
    
    
    // 生成點雲
    
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
        // 如果機器慢，把後面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < leftImg.rows; v++)
        for (int u = 0; u < leftImg.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) 
                continue;
            Vector4d point(0, 0, 0, leftImg.at<uchar>(v, u) / 255.0); // 前三維爲xyz,第四維爲顏色
            // 根據雙目模型計算 point 的位置
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b/(disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;
            pointcloud.push_back(point);
        }
        
    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    // 畫出點雲
    showPointCloud(pointcloud);
    return 0;
}


void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) 
{
    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
    while (pangolin::ShouldQuit() == false) 
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    
    return;
}

5.運行方法：

在build目錄下，打開終端

./imagebinoculartest 

結果如下：

原始左右兩圖：

SGBM計算得到的視差圖：

根據視差圖得到的點雲圖：

可以用鼠標左鍵點擊，讓它旋轉，讓實驗效果更突出。