双目3D视觉

@

目录

• 一、双目模型
• 二、像素匹配方法
  • 2.1 SAD法
  • 2.2 OpenCV自带的API

一、双目模型

这里只讨论最简单的情况,两个相机内参相同, 两个成像平面在相同平面上,同一水平高度上

二、像素匹配方法

2.1 SAD法

结果如下：

#!/usr/bin/python3
# coding=utf-8
import numpy as np
import cv2
import matplotlib.pyplot as plt

# 从CSV读取灰度图imgL和imgR
print('loading image from CSV file')
imgL = np.genfromtxt('aL_gray.csv',delimiter=',').astype(np.float32)
imgR = np.genfromtxt('aR_gray.csv',delimiter=',').astype(np.float32)
HGT,WID=imgL.shape  # 图像尺寸

D=40    # 像素匹配搜索是，最大偏移量
W=7     # 像素匹配搜索时，窗口大小

# 构建一系列平移后的图img_shift
img_shift=np.zeros((D,HGT,WID))
for d in range(D):
    img_shift[d,:,:]=np.roll(imgR, d,axis=1)

# 计算左图和一系列平移后的右图的差,差别取绝对值
img_diff=np.abs(img_shift-imgL)

# 对图像差计算窗口平滑
for n in range(img_diff.shape[0]):
    img_diff[n,:,:]=cv2.boxFilter(img_diff[n,:,:],-1,(W,W))

# 逐个像素求最匹配的平移量
imgD=np.zeros((HGT,WID))
imgD=np.argmin(img_diff,axis=0)
np.savetxt('match.csv', imgD, fmt='%d', delimiter=',', newline='
') # 保存为csv文件

# 显示结果
imgD=np.genfromtxt('match.csv', delimiter=',').astype(np.float32)
if False:
    plt.clf()
    plt.subplot(1,3,1);plt.imshow(cv2.cvtColor(imgL.astype(np.uint8),cv2.COLOR_BGR2RGB))
    plt.subplot(1,3,2);plt.imshow(cv2.cvtColor(imgR.astype(np.uint8),cv2.COLOR_BGR2RGB))
    plt.subplot(1,3,3);plt.imshow(imgD)
    plt.show()
plt.imshow(imgD,cmap='jet')
plt.show()

2.2 OpenCV自带的API

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "/home/qian/slambook2/ch5/stereo/left.png";
string right_file = "/home/qian/slambook2/ch5/stereo/right.png";

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud); //使用eigen库中的变量Vector、matrix等时需要这样写

int main(int argc, char **argv) {

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    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(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Vector4d point(0, 0, 0, left.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) {

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