如何使用C++语言实现双目三维重建系统项目

如何使用C++语言实现双目三维重建系统项目

双目标定，立体校正，双目测距，三维重建

该项目旨在了解三维重建流程：包括相机标定，立体匹配，深度计算等等

代码包含:
支持双USB连接线的双目摄像头
（如果是单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示，需要分成立体像对)
支持单目相机标定
支持双目相机标定，无需 Matlab标定
支持使用WLS滤波器对视差图进行滤波
支持双目测距，误差在lcm内(鼠标点击图像即可获得其深度距离)
支持Open3D和PCL点云显示

使用 C++ 实现双目三维重建系统项目

双目立体视觉（Stereo Vision）是一种通过两个摄像头从不同角度拍摄同一场景，利用视差信息来估计深度图并进行三维重建的技术。本项目将使用 C++ 和 OpenCV 库实现一个基础的双目三维重建系统。

---

🧠 一、基本原理

1. 双目标定（Stereo Calibration）

• 获取相机内参（焦距、畸变系数等）和外参（旋转和平移矩阵）。
• 计算本质矩阵（Essential Matrix）和基础矩阵（Fundamental Matrix）。

2. 立体校正（Rectification）

• 对左右图像进行透视变换，使图像行对齐，便于后续匹配。

3. 视差计算

• 使用 SGBM（Semi-Global Block Matching） 或 BM（Block Matching）算法计算视差图。

4. 深度重建

• 利用视差图和相机参数生成点云数据，实现三维空间重建。

---

🛠️ 二、开发环境要求

• 编程语言：C++
• 开发工具：Visual Studio / g++ / CLion
• 图像库：OpenCV（版本 >= 3.4）

—

📦 三、主要模块结构

stereo_reconstruction/
├── calibration/           # 标定文件
├── images/                # 左右图像
├── include/
│   └── stereo_utils.h     # 头文件
├── src/
│   ├── main.cpp             # 主程序入口
│   └── stereo_utils.cpp     # 功能实现
├── CMakeLists.txt
└── README.md

---

📄 四、核心代码示例

1. stereo_utils.h —— 函数声明

#ifndef STEREO_UTILS_H
#define STEREO_UTILS_H

#include <opencv2/opencv.hpp>

void stereo_calibrate(const std::string& left_img_dir, const std::string& right_img_dir,
                       cv::Size board_size, float square_size,
                       cv::Mat& K1, cv::Mat& D1, cv::Mat& K2, cv::Mat& D2,
                       cv::Mat& R, cv::Mat& T, cv::Mat& E, cv::Mat& F);

void stereo_rectify(const cv::Mat& K1, const cv::Mat& D1, const cv::Mat& K2, const cv::Mat& D2,
                    const cv::Mat& R, const cv::Mat& T, cv::Size img_size,
                    cv::Mat& R1, cv::Mat& R2, cv::Mat& P1, cv::Mat& P2, cv::Mat& Q);

void compute_disparity(const cv::Mat& left, const cv::Mat& right,
                       cv::Mat& disparity, cv::Mat& coloredDisparity);

void reconstruct_3d(const cv::Mat& disparity, const cv::Mat& Q, cv::Mat& points_3d);

#endif // STEREO_UTILS_H

---

2. stereo_utils.cpp —— 功能实现

#include "stereo_utils.h"
#include <iostream>
#include <vector>

using namespace cv;
using namespace std;

void stereo_calibrate(const string& left_dir, const string& right_dir,
                      Size board_size, float square_size,
                      Mat& K1, Mat& D1, Mat& K2, Mat& D2,
                      Mat& R, Mat& T, Mat& E, Mat& F) {
    vector<vector<Point3f>> object_points;
    vector<vector<Point2f>> image_points1, image_points2;

    for (int i = 0; i < 10; ++i) {
        string left_path = left_dir + "/left_" + to_string(i) + ".jpg";
        string right_path = right_dir + "/right_" + to_string(i) + ".jpg";

        Mat img1 = imread(left_path, IMREAD_GRAYSCALE);
        Mat img2 = imread(right_path, IMREAD_GRAYSCALE);

        vector<Point2f> corners1, corners2;
        bool found1 = findChessboardCorners(img1, board_size, corners1);
        bool found2 = findChessboardCorners(img2, board_size, corners2);

        if (found1 && found2) {
            TermCriteria criteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.001);
            cornerSubPix(img1, corners1, Size(5, 5), Size(-1, -1), criteria);
            cornerSubPix(img2, corners2, Size(5, 5), Size(-1, -1), criteria);

            image_points1.push_back(corners1);
            image_points2.push_back(corners2);

            vector<Point3f> obj;
            for (int r = 0; r < board_size.height; ++r)
                for (int c = 0; c < board_size.width; ++c)
                    obj.push_back(Point3f(float(c * square_size), float(r * square_size), 0));
            object_points.push_back(obj);
        }
    }

    stereoCalibrate(object_points, image_points1, image_points2,
                    K1, D1, K2, D2, Size(), R, T, E, F,
                    CALIB_FIX_INTRINSIC, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));

    cout << "R:\n" << R << "\nT:\n" << T << endl;
}

void stereo_rectify(const Mat& K1, const Mat& D1, const Mat& K2, const Mat& D2,
                    const Mat& R, const Mat& T, Size img_size,
                    Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q) {
    stereoRectify(K1, D1, K2, D2, img_size, R, T, R1, R2, P1, P2, Q);
}

void compute_disparity(const Mat& left, const Mat& right,
                       Mat& disparity, Mat& coloredDisparity) {
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
    sgbm->setNumDisparities(16 * 5);
    sgbm->setBlockSize(5);
    sgbm->setP1(8 * 3 * 5 * 5);
    sgbm->setP2(32 * 3 * 5 * 5);
    sgbm->setMinDisparity(0);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY);

    Mat disp16S;
    sgbm->compute(left, right, disp16S);
    normalize(disp16S, disparity, 0, 255, NORM_MINMAX, CV_8U);

    applyColorMap(disparity, coloredDisparity, COLORMAP_JET);
}

void reconstruct_3d(const Mat& disparity, const Mat& Q, Mat& points_3d) {
    reprojectImageTo3D(disparity, points_3d, Q, true);
}

---

3. main.cpp —— 主函数逻辑

#include "stereo_utils.h"

int main() {
    // 相机标定参数
    Size board_size(9, 6);         // 棋盘格角点数
    float square_size = 25.0f;     // 单位 mm

    Mat K1 = Mat::eye(3, 3, CV_64F), D1 = Mat::zeros(8, 1, CV_64F);
    Mat K2 = Mat::eye(3, 3, CV_64F), D2 = Mat::zeros(8, 1, CV_64F);
    Mat R, T, E, F;

    // 1. 双目标定
    stereo_calibrate("calibration/left", "calibration/right",
                     board_size, square_size, K1, D1, K2, D2, R, T, E, F);

    // 2. 立体校正参数
    Mat R1, R2, P1, P2, Q;
    stereo_rectify(K1, D1, K2, D2, R, T, Size(640, 480), R1, R2, P1, P2, Q);

    // 3. 加载左右图像
    Mat left = imread("images/left_0.jpg", IMREAD_GRAYSCALE);
    Mat right = imread("images/right_0.jpg", IMREAD_GRAYSCALE);

    // 4. 计算视差图
    Mat disparity, coloredDisparity;
    compute_disparity(left, right, disparity, coloredDisparity);

    imshow("Disparity", coloredDisparity);

    // 5. 三维重建
    Mat points_3d;
    reconstruct_3d(disparity, Q, points_3d);

    waitKey(0);
    return 0;
}

---

🧪 五、运行步骤

1. 准备标定图像

在 calibration/left 和 calibration/right 下准备至少 10 组对应的棋盘格图像。

2. 构建与编译（CMake 示例）

cmake_minimum_required(VERSION 3.10)
project(stereo_reconstruction)

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(stereo_reconstruction src/main.cpp src/stereo_utils.cpp)

target_link_libraries(stereo_reconstruction ${OpenCV_LIBS})

3. 执行程序

mkdir build && cd build
cmake ..
make
./stereo_reconstruction

---

🎯 六、输出结果

• 显示视差图（彩色映射）；
• 得到三维点云数据（可用于 PCL 显示或保存为 .ply 文件）；
• 后续可结合 PCL（Point Cloud Library） 进行可视化。

---

🧩 七、扩展功能建议

• 使用 PCL 显示点云；
• 支持实时视频流输入（USB摄像头或ZED相机）；
• 增加 GUI 界面（Qt / OpenCV HighGui）；
• 使用 CUDA 加速视差计算；
• 添加特征匹配（ORB/SIFT）辅助稀疏点云提取；
• 支持多尺度重建、纹理映射。

---