【双目视觉】立体匹配

双目视觉和RGBD视觉

双目视觉代码

#include <iostream>
#include <fstream>
#include <vector>
//unistd.h为Linux/Unix系统中内置头文件，包含了许多系统服务的函数原型，例如read函数、write函数和getpid函数等。
//其作用相当于windows操作系统的"windows.h"，是操作系统为用户提供的统一API接口，方便调用系统提供的一些服务。
#include <unistd.h>
#include <eigen3/Eigen/Core>
#include <pangolin/pangolin.h>
#include <opencv2/opencv.hpp>
using namespace std;
const string file_left = "/home/wpf/Test/study_cpp/chapter41/data/left.png";
const string file_right = "/home/wpf/Test/study_cpp/chapter41/data/right.png";
void showpointCloud(vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> &pointCloud);
int main() {
    
    

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;
    //读入灰色图像
    cv::Mat img_left = cv::imread(file_left,cv::IMREAD_GRAYSCALE);
    cv::Mat img_right = cv::imread(file_right,cv::IMREAD_GRAYSCALE);
    assert(!img_left.empty() && !img_right.empty());
    cv::imshow("left_image",img_left);
    cv::imshow("right_image", img_right);

    //调用固定的函数来计算视差
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create( 0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
    cv::Mat disparity_sgbm,disparity;
    sgbm->compute(img_left,img_right,disparity_sgbm);
    disparity_sgbm.convertTo(disparity,CV_32F,1.0/16.0f);
    //产生点云
    vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>> pointCloud;
    //图像上太近的点和太远的点不考虑，在10——96之间的数据进行分析
    for (int v = 0; v < img_left.rows; ++v) {
    
    
        for (int u = 0; u < img_left.cols; ++u) {
    
    
            if(disparity.at<float>(v,u)<=10.0||disparity.at<float>(v,u)>=96.0) continue;
            //初始化一个点
            Eigen::Vector4d point(0,0,0,img_left.at<uchar>(v,u)/255);//归一化
            //计算深度
            double depth = fx*b/disparity.at<float>(v,u);
            double X = ((u-cx)*disparity.at<float>(v,u))/fx;
            double Y = ((v-cy)*disparity.at<float>(v,u))/fy;
            point[0] = X;
            point[1] = Y;
            point[2] = depth;
            pointCloud.emplace_back(point);
        }
    }
    //视差图
    cv::imshow("disparity",disparity/96);
    cv::waitKey(0);
    showpointCloud(pointCloud);
    std::cout << "Hi,wpf!You're the best!" << std::endl;
    return 0;
}
void showpointCloud(vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> &pointcloud) {
    
    
    if (pointcloud.empty()) {
    
    
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    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;
}

特别说明。这里的视差计算方法涉及立体匹配的知识
参考链接：
立体匹配基本原理
立体匹配方法比较
C++语法，

有一个智能指针，cv::Ptr<>

结果如下图所示