双目立体视觉各流程及原理详解：C++ / OpenCV 实现

摘要

双目立体视觉是一种典型的三维重建方法，其基本思想是利用两个相机从不同视角观察同一场景，通过左右图像中同名点的位置差异计算视差，再根据三角测量原理恢复物体的深度信息和三维坐标。本文从双目视觉的基本原理出发，系统讲解相机成像模型、双目标定、极线约束、极线校正、立体匹配、视差计算、深度恢复和点云生成等核心流程，并结合 OpenCV C++ 给出完整实现代码，适合用于学习双目三维重建的整体流程。

关键词

双目立体视觉、三维重建、极线约束、极线校正、视差图、StereoSGBM、OpenCV、C++

---

1. 双目立体视觉概述

双目立体视觉的核心目标是：

通过两个相机拍摄同一场景，寻找左右图像中的同名点，计算视差，并根据三角测量恢复三维坐标。

它模仿的是人眼观察世界的方式。左眼和右眼看到的图像存在细微差异，大脑可以根据这种差异判断物体远近。双目相机也是类似原理。

一个典型的双目系统如下：

左相机  ----------------------  右相机
   |                                |
   |                                |
   ↓                                ↓
 左图像                            右图像

同一个空间点 (P) 会分别投影到左右图像中：

$$P\to p_L(u_L,v_L)$$

$$P\to p_R(u_R,v_R)$$

如果能够找到左右图像中的同名点 (p_L) 和 (p_R)，就可以恢复空间点 (P) 的三维坐标。

---

2. 双目立体视觉整体流程

完整的双目立体视觉流程如下：

双目图像采集
    ↓
双目标定
    ↓
畸变校正
    ↓
极线校正
    ↓
立体匹配
    ↓
视差图计算
    ↓
深度图计算
    ↓
三维点云重建
    ↓
后处理与测量

对应到 OpenCV C++ 中，常用函数如下：

流程	作用	OpenCV C++ 函数
单目标定	求每个相机的内参和畸变参数	calibrateCamera()
双目标定	求左右相机之间的旋转和平移	stereoCalibrate()
极线校正	让左右图像同名点位于同一行	stereoRectify()
映射表生成	生成畸变校正和极线校正映射表	initUndistortRectifyMap()
图像重映射	得到校正后的左右图像	remap()
立体匹配	计算视差图	StereoBM / StereoSGBM
三维重建	由视差图恢复三维点	reprojectImageTo3D()

---

3. 相机成像模型

双目视觉的基础是针孔相机模型。

空间点：

$$P=(X,Y,Z)$$

投影到图像平面上的像素点：

$$p=(u,v)$$

满足：

$$u=f_x\frac{X}{Z}+c_x$$

$$v=f_y\frac{Y}{Z}+c_y$$

其中：

参数	含义
(X, Y, Z)	空间点在相机坐标系下的三维坐标
(u, v)	图像像素坐标
(f_x, f_y)	像素单位下的焦距
(c_x, c_y)	图像主点坐标
(Z)	深度

相机内参矩阵为：

$$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

在 OpenCV C++ 中，内参矩阵通常用 cv::Mat 表示：

cv::Mat K = (cv::Mat_<double>(3, 3) <<
    fx, 0,  cx,
    0,  fy, cy,
    0,  0,  1
);

---

4. 双目标定

4.1 双目标定的目的

双目标定的目的是求出左右相机的内参、畸变参数以及两个相机之间的外参。

主要包括：

参数	含义
(K_1)	左相机内参
(D_1)	左相机畸变参数
(K_2)	右相机内参
(D_2)	右相机畸变参数
(R)	右相机相对于左相机的旋转矩阵
(T)	右相机相对于左相机的平移向量
(E)	本质矩阵
(F)	基础矩阵

如果以左相机坐标系作为参考，则右相机坐标系中的点和左相机坐标系中的点满足：

$$P_R=R{P}_L+T$$

其中：

• (P_L)：空间点在左相机坐标系下的坐标；
• (P_R)：空间点在右相机坐标系下的坐标；
• (R)：右相机相对于左相机的旋转矩阵；
• (T)：右相机相对于左相机的平移向量。

平移向量 (T) 的模长就是双目基线：

$$B=\Vert T\Vert$$

---

4.2 标定图像采集要求

双目标定通常使用棋盘格标定板。采集标定图像时需要注意：

1. 左右相机必须同时看到完整棋盘格；
2. 左右图像必须一一对应；
3. 棋盘格应覆盖图像中心、边缘和四个角；
4. 标定板应有不同距离、不同角度和不同姿态；
5. 标定过程中左右相机的相对位置不能发生变化；
6. 图像应清晰，避免运动模糊和严重反光。

---

4.3 棋盘格角点检测示例

假设棋盘格内角点数量为 (9 \times 6)，每个格子边长为 25 mm：

cv::Size boardSize(9, 6);
float squareSize = 25.0f;

生成棋盘格三维世界坐标：

std::vector<cv::Point3f> createObjectPoints(cv::Size boardSize, float squareSize)
{
    std::vector<cv::Point3f> objectPoints;

    for (int y = 0; y < boardSize.height; y++)
    {
        for (int x = 0; x < boardSize.width; x++)
        {
            objectPoints.emplace_back(
                x * squareSize,
                y * squareSize,
                0.0f
            );
        }
    }

    return objectPoints;
}

检测左右图像中的角点：

cv::Mat imgL = cv::imread("left_01.png", cv::IMREAD_GRAYSCALE);
cv::Mat imgR = cv::imread("right_01.png", cv::IMREAD_GRAYSCALE);

std::vector<cv::Point2f> cornersL;
std::vector<cv::Point2f> cornersR;

bool foundL = cv::findChessboardCorners(imgL, boardSize, cornersL);
bool foundR = cv::findChessboardCorners(imgR, boardSize, cornersR);

if (foundL && foundR)
{
    cv::cornerSubPix(
        imgL,
        cornersL,
        cv::Size(11, 11),
        cv::Size(-1, -1),
        cv::TermCriteria(
            cv::TermCriteria::EPS + cv::TermCriteria::COUNT,
            30,
            0.01
        )
    );

    cv::cornerSubPix(
        imgR,
        cornersR,
        cv::Size(11, 11),
        cv::Size(-1, -1),
        cv::TermCriteria(
            cv::TermCriteria::EPS + cv::TermCriteria::COUNT,
            30,
            0.01
        )
    );
}

---

4.4 双目标定 C++ 示例

实际工程中，一般先分别对左右相机做单目标定，再用 stereoCalibrate() 进行双目标定。

std::vector<std::vector<cv::Point3f>> objectPoints;
std::vector<std::vector<cv::Point2f>> imagePointsL;
std::vector<std::vector<cv::Point2f>> imagePointsR;

cv::Mat K1, D1;
cv::Mat K2, D2;
cv::Mat R, T, E, F;

double rms = cv::stereoCalibrate(
    objectPoints,
    imagePointsL,
    imagePointsR,
    K1,
    D1,
    K2,
    D2,
    imageSize,
    R,
    T,
    E,
    F,
    cv::CALIB_FIX_INTRINSIC,
    cv::TermCriteria(
        cv::TermCriteria::COUNT + cv::TermCriteria::EPS,
        100,
        1e-5
    )
);

std::cout << "Stereo calibration RMS error = " << rms << std::endl;
std::cout << "R = " << R << std::endl;
std::cout << "T = " << T << std::endl;

其中 rms 是重投影误差。一般来说，误差越小，标定结果越好。

---

5. 极线约束原理

5.1 为什么需要极线约束？

在双目匹配中，如果没有任何约束，左图中的一个点需要在右图整幅图像中寻找对应点：

左图一个点 → 右图整幅图搜索

这样不仅计算量大，而且容易出现误匹配。

极线约束可以把搜索范围从二维图像区域缩小到一条直线：

左图一个点 → 右图对应极线搜索

---

5.2 极线约束公式

左右图像中的同名点满足：

$$p_R^{T}F{p}_L=0$$

其中：

符号	含义
(p_L)	左图像点的齐次坐标
(p_R)	右图像点的齐次坐标
(F)	基础矩阵

如果：

$$p_L=\left[\begin{array}{c}{u}_L\\ v_L\\ 1\end{array}\right]$$

$$p_R=\left[\begin{array}{c}{u}_R\\ v_R\\ 1\end{array}\right]$$

则正确匹配点必须满足：

$$p_R^{T}F{p}_L=0$$

已知左图点 (p_L)，可以计算它在右图中的极线：

$$l_R=F{p}_L$$

如果：

$$l_R=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$$

那么右图极线方程为：

$$au+bv+c=0$$

右图中的匹配点必须落在这条直线上。

---

6. 极线校正

6.1 极线校正的目的

虽然极线约束可以把匹配点限制在一条线上，但原始图像中的极线可能是倾斜的。为了让匹配更方便，需要进行极线校正。

极线校正的目标是使左右图像满足：

$$v_L\approx v_R$$

也就是说：

同一个空间点在左右图像中位于同一行。

校正前：

左图点 → 右图倾斜极线搜索

校正后：

左图点 → 右图同一行水平搜索

这样，双目匹配就从二维搜索变成了一维搜索。

---

6.2 stereoRectify

OpenCV 中使用 stereoRectify() 计算极线校正参数：

cv::Mat R1, R2;
cv::Mat P1, P2;
cv::Mat Q;
cv::Rect validROI1, validROI2;

cv::stereoRectify(
    K1,
    D1,
    K2,
    D2,
    imageSize,
    R,
    T,
    R1,
    R2,
    P1,
    P2,
    Q,
    cv::CALIB_ZERO_DISPARITY,
    0,
    imageSize,
    &validROI1,
    &validROI2
);

输出参数说明：

参数	含义
R1	左相机校正旋转矩阵
R2	右相机校正旋转矩阵
P1	左相机校正后的投影矩阵
P2	右相机校正后的投影矩阵
Q	视差到三维坐标的重投影矩阵
validROI1	左图有效区域
validROI2	右图有效区域

---

6.3 initUndistortRectifyMap

根据 stereoRectify() 得到的参数生成映射表：

cv::Mat mapLx, mapLy;
cv::Mat mapRx, mapRy;

cv::initUndistortRectifyMap(
    K1,
    D1,
    R1,
    P1,
    imageSize,
    CV_32FC1,
    mapLx,
    mapLy
);

cv::initUndistortRectifyMap(
    K2,
    D2,
    R2,
    P2,
    imageSize,
    CV_32FC1,
    mapRx,
    mapRy
);

这一步得到的是从校正图像到原始图像的像素映射关系。

---

6.4 remap

使用 remap() 得到校正后的左右图像：

cv::Mat rectL, rectR;

cv::remap(
    imgL,
    rectL,
    mapLx,
    mapLy,
    cv::INTER_LINEAR
);

cv::remap(
    imgR,
    rectR,
    mapRx,
    mapRy,
    cv::INTER_LINEAR
);

校正后的图像满足：

同一个空间点在左右图像中位于同一行

这一步是立体匹配之前非常重要的预处理步骤。

---

7. 视差与深度

完成极线校正后，同名点满足：

$$v_L=v_R$$

因此只需要计算横坐标差异。

视差定义为：

$$d=u_L-u_R$$

其中：

参数	含义
(d)	视差
(u_L)	左图像点横坐标
(u_R)	右图像点横坐标

理想平行双目模型中，深度和视差满足：

$$Z=\frac{fB}{d}$$

其中：

参数	含义
(Z)	深度
(f)	焦距，通常使用像素单位
(B)	双目基线
(d)	视差

由该公式可以看出：

• 视差越大，物体越近；
• 视差越小，物体越远；
• 视差接近 0 时，深度趋向无穷大；
• 远距离目标的深度误差会明显增大。

---

8. 立体匹配

8.1 立体匹配的目标

立体匹配是双目立体视觉中最关键的一步。

它的目标是：

对左图像中的每一个像素，在右图像中找到对应的同名点，从而计算该像素的视差。

经过极线校正后，左右图像中的同名点位于同一行：

$$v_L\approx v_R$$

因此，对于左图像中的像素点：

$$p_L=(u_L,v)$$

只需要在右图像同一行搜索：

$$p_R=(u_R,v)$$

视差定义为：

$$d=u_L-u_R$$

所以右图中的候选匹配点可以写成：

$$p_R=(u_L-d,v)$$

立体匹配的本质就是在一系列候选视差 (d) 中，找到最可能的匹配位置。

---

8.2 常见立体匹配方法

常见立体匹配算法包括：

方法	基本思想	特点
SAD	计算窗口灰度绝对差之和	简单快速，但对光照变化敏感
SSD	计算窗口灰度平方差之和	数学形式简单，但对异常值敏感
NCC	计算窗口归一化互相关	对亮度变化更鲁棒，但计算量较大
BM	基于局部窗口的块匹配	速度快，但精度一般
SGBM	半全局块匹配	精度和速度较均衡，工程中常用
Census + SGM	基于局部灰度排序关系	对光照变化较鲁棒
Graph Cut	全局优化	精度较高，但计算量大
深度学习方法	使用神经网络学习匹配关系	精度高，但依赖训练数据和算力

立体匹配一般可以分为以下步骤：

匹配代价计算
    ↓
代价聚合
    ↓
视差选择
    ↓
视差后处理

其中，NCC、SAD、SSD 等方法通常用于计算匹配代价。

---

8.3 SAD 匹配代价

SAD 是 Sum of Absolute Differences，即绝对差值和。

对于左图窗口 (I_L) 和右图候选窗口 (I_R)，SAD 定义为：

$$SAD=\sum _{i,j}|I_L(i,j)-I_R(i,j)|$$

SAD 值越小，说明两个窗口越相似。

因此，使用 SAD 进行立体匹配时，选择 SAD 最小的视差：

$$d(u,v)=\arg \underset{d}{\min }SAD(u,v,d)$$

SAD 的优点是计算简单、速度快。

缺点是直接比较灰度值，因此对光照变化、曝光差异较敏感。

---

8.4 SSD 匹配代价

SSD 是 Sum of Squared Differences，即平方差和。

其定义为：

$$SSD=\sum _{i,j}{\left(I_L(i,j)-I_R(i,j)\right)}^2$$

SSD 同样是代价函数，值越小表示两个窗口越相似：

$$d(u,v)=\arg \underset{d}{\min }SSD(u,v,d)$$

SSD 相比 SAD 对较大的灰度差更加敏感，因此在存在噪声或异常点时，可能受到更大影响。

---

8.5 NCC 立体匹配

8.5.1 NCC 基本思想

NCC 是 Normalized Cross-Correlation，即归一化互相关。

它用于衡量两个局部窗口的相似程度。

在双目立体匹配中，对于左图像中的像素点 ((u,v))，以该点为中心取一个窗口：

$$W_L(u,v)$$

然后在右图像同一行搜索候选视差 (d)，对应右图窗口为：

$$W_R(u-d,v)$$

NCC 通过计算这两个窗口的归一化相关系数来判断它们是否匹配。

其基本流程为：

左图窗口 W_L(u, v)
        ↓
右图同一行搜索候选窗口 W_R(u-d, v)
        ↓
计算每个候选窗口的 NCC
        ↓
选择 NCC 最大的位置
        ↓
得到该像素视差 d

---

8.5.2 NCC 公式

设左图窗口为 (I_L)，右图窗口为 (I_R)，窗口大小为 (N \times N)。

NCC 定义为：

$$NCC=\frac{{\sum }_{i,j}\left(I_L(i,j)-{\bar{I}}_L\right)\left(I_R(i,j)-{\bar{I}}_R\right)}{\sqrt{{\sum }_{i,j}{\left(I_L(i,j)-{\bar{I}}_L\right)}^2}\sqrt{{\sum }_{i,j}{\left(I_R(i,j)-{\bar{I}}_R\right)}^2}}$$

其中：

符号	含义
(I_L(i,j))	左图窗口中的像素灰度
(I_R(i,j))	右图窗口中的像素灰度
(\bar{I}_L)	左图窗口平均灰度
(\bar{I}_R)	右图窗口平均灰度
(NCC)	两个窗口的归一化相关系数

NCC 的取值范围为：

$$-1\le NCC\le 1$$

其含义如下：

NCC 值	含义
接近 1	两个窗口非常相似
接近 0	两个窗口相关性较弱
接近 -1	两个窗口灰度变化趋势相反

在立体匹配中，NCC 是相似度指标，因此选择 NCC 最大的视差：

$$d(u,v)=\arg \underset{d}{\max }NCC(u,v,d)$$

---

8.5.3 NCC 为什么对亮度变化更鲁棒？

SAD 和 SSD 都是直接比较灰度值：

$$SAD=\sum |I_L-I_R|$$

$$SSD=\sum (I_L-I_R{)}^2$$

如果左右相机曝光不同，例如：

左图窗口整体偏暗
右图窗口整体偏亮

那么 SAD 和 SSD 可能会认为两个窗口差异很大。

而 NCC 会先减去窗口均值，再除以标准差，因此它更关注窗口内部灰度变化趋势，而不是绝对灰度值。

所以 NCC 对以下情况更鲁棒：

• 左右相机曝光略有不同；
• 图像整体亮度变化；
• 局部灰度线性变化。

---

8.5.4 NCC 立体匹配 C++ 示例

下面给出一个简单的 NCC 立体匹配实现。

该代码假设左右图像已经完成极线校正。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <cmath>

using namespace cv;
using namespace std;

/**
 * 计算两个窗口的 NCC
 */
double computeNCC(
    const Mat& left,
    const Mat& right,
    int xL,
    int y,
    int xR,
    int windowSize
)
{
    int half = windowSize / 2;

    double meanL = 0.0;
    double meanR = 0.0;
    int count = 0;

    // 1. 计算左右窗口均值
    for (int dy = -half; dy <= half; dy++)
    {
        for (int dx = -half; dx <= half; dx++)
        {
            int yy = y + dy;
            int xl = xL + dx;
            int xr = xR + dx;

            meanL += left.at<uchar>(yy, xl);
            meanR += right.at<uchar>(yy, xr);
            count++;
        }
    }

    meanL /= count;
    meanR /= count;

    double numerator = 0.0;
    double denomL = 0.0;
    double denomR = 0.0;

    // 2. 计算 NCC 分子和分母
    for (int dy = -half; dy <= half; dy++)
    {
        for (int dx = -half; dx <= half; dx++)
        {
            int yy = y + dy;
            int xl = xL + dx;
            int xr = xR + dx;

            double valL = left.at<uchar>(yy, xl) - meanL;
            double valR = right.at<uchar>(yy, xr) - meanR;

            numerator += valL * valR;
            denomL += valL * valL;
            denomR += valR * valR;
        }
    }

    double denominator = sqrt(denomL * denomR);

    // 如果窗口纹理太弱，分母接近 0，说明该区域不适合匹配
    if (denominator < 1e-6)
        return -1.0;

    return numerator / denominator;
}

/**
 * 基于 NCC 的立体匹配
 */
Mat stereoMatchingNCC(
    const Mat& leftGray,
    const Mat& rightGray,
    int maxDisparity,
    int windowSize
)
{
    CV_Assert(leftGray.type() == CV_8UC1);
    CV_Assert(rightGray.type() == CV_8UC1);
    CV_Assert(leftGray.size() == rightGray.size());
    CV_Assert(windowSize % 2 == 1);

    int rows = leftGray.rows;
    int cols = leftGray.cols;
    int half = windowSize / 2;

    Mat disparity = Mat::zeros(rows, cols, CV_32F);

    for (int y = half; y < rows - half; y++)
    {
        for (int x = half + maxDisparity; x < cols - half; x++)
        {
            double bestNCC = -1.0;
            int bestDisparity = 0;

            for (int d = 0; d <= maxDisparity; d++)
            {
                int xR = x - d;

                if (xR - half < 0)
                    continue;

                double ncc = computeNCC(
                    leftGray,
                    rightGray,
                    x,
                    y,
                    xR,
                    windowSize
                );

                if (ncc > bestNCC)
                {
                    bestNCC = ncc;
                    bestDisparity = d;
                }
            }

            disparity.at<float>(y, x) = static_cast<float>(bestDisparity);
        }
    }

    return disparity;
}

int main()
{
    Mat left = imread("rectified_left.png", IMREAD_GRAYSCALE);
    Mat right = imread("rectified_right.png", IMREAD_GRAYSCALE);

    if (left.empty() || right.empty())
    {
        cerr << "图像读取失败！" << endl;
        return -1;
    }

    int maxDisparity = 128;
    int windowSize = 9;

    Mat disparity = stereoMatchingNCC(
        left,
        right,
        maxDisparity,
        windowSize
    );

    Mat disparityVis;
    normalize(disparity, disparityVis, 0, 255, NORM_MINMAX, CV_8U);

    imshow("NCC Disparity", disparityVis);
    imwrite("ncc_disparity.png", disparityVis);

    waitKey(0);

    return 0;
}

---

8.5.5 NCC 代码核心解释

对于左图像点 ((x,y))，右图候选点为：

int xR = x - d;

对应公式：

$$u_R=u_L-d$$

视差为：

$$d=u_L-u_R$$

由于 NCC 是相似度指标，因此取最大值对应的视差：

if (ncc > bestNCC)
{
    bestNCC = ncc;
    bestDisparity = d;
}

即：

$$d=\arg \underset{d}{\max }NCC$$

如果窗口区域纹理很弱，例如白墙、纯色桌面等区域，则窗口内灰度几乎不变化，此时：

$$I(i,j)-\bar{I}\approx 0$$

导致 NCC 分母接近 0。代码中使用：

if (denominator < 1e-6)
    return -1.0;

避免除零错误。

但从匹配本质上看，低纹理区域缺少可区分特征，因此 NCC、SAD、SSD 等局部匹配方法都难以稳定匹配。

---

8.5.6 NCC 的优缺点

NCC 的优点：

优点	说明
对亮度变化较鲁棒	减去均值并归一化标准差
原理直观	本质上比较两个窗口的灰度变化趋势
比 SAD / SSD 更稳定	曝光略有差异时仍可能获得较好匹配
适合理解立体匹配原理	公式清晰，易于实现

NCC 的缺点：

缺点	说明
计算量较大	每个像素、每个视差都要计算窗口均值和方差
低纹理区域不稳定	窗口方差接近 0
边缘容易模糊	窗口跨越深度边界
遮挡区域无法处理	没有真实对应点
重复纹理容易误匹配	多个候选位置 NCC 都可能较高

---

8.5.7 NCC 窗口大小选择

窗口大小对 NCC 匹配效果影响很大。

窗口大小	特点	适用场景
(3 \times 3)	边缘保持较好，但抗噪能力弱	纹理丰富、图像清晰
(5 \times 5)	速度较快，稳定性一般	简单场景
(7 \times 7)	精度和稳定性较均衡	常规双目匹配
(9 \times 9)	抗噪能力更强	噪声较大或纹理较弱
(11 \times 11) 以上	更平滑，但边缘容易模糊	平面区域较多的场景

一般可以从下面参数开始调试：

int windowSize = 7;

或者：

int windowSize = 9;

---

8.6 BM 块匹配

BM 是 Block Matching，即块匹配。

其基本思想是：

以左图某个像素为中心取一个窗口，在右图同一行搜索最相似的窗口。

基本流程如下：

取左图窗口
    ↓
在右图同一行滑动搜索
    ↓
计算 SAD / SSD / NCC 等匹配代价
    ↓
选择代价最小或相似度最大的候选位置
    ↓
得到视差

BM 的优点：

• 原理简单；
• 速度快；
• 易于实现。

BM 的缺点：

• 对低纹理区域效果差；
• 对重复纹理容易误匹配；
• 物体边缘处容易出现视差混合；
• 没有全局约束，视差图通常比较粗糙。

OpenCV 中可以使用 StereoBM 实现 BM：

cv::Ptr<cv::StereoBM> bm = cv::StereoBM::create(128, 9);

cv::Mat disparity16S;
bm->compute(grayL, grayR, disparity16S);

---

8.7 SGBM 半全局块匹配

8.7.1 SGBM 基本思想

SGBM 是 Semi-Global Block Matching，即半全局块匹配。

相比普通 BM，SGBM 不仅考虑局部窗口匹配代价，还引入了半全局路径约束。

它的核心思想是：

1. 计算每个像素在不同视差下的匹配代价；
2. 沿多个方向进行路径代价聚合；
3. 使用平滑项约束相邻像素的视差变化；
4. 在物体边缘处允许视差突变。

SGBM 相比 BM 更稳定，是 OpenCV 中工程上非常常用的立体匹配方法。

---

8.7.2 SGBM 能量函数

SGBM 的能量函数可以理解为：

$$E(D)=\sum _{p}C(p,D_p)+\sum _{q\in N_p}{P}_1[|D_p-D_q|=1]+\sum _{q\in N_p}{P}_2[|D_p-D_q|>1]$$

其中：

参数	含义
(D_p)	像素 (p) 的视差
(C(p,D_p))	像素匹配代价
(P_1)	小视差变化惩罚
(P_2)	大视差变化惩罚

直观理解：

• 相邻像素视差应该尽量平滑；
• 小的视差变化允许存在；
• 大的视差变化需要更大惩罚；
• 在真实物体边缘处允许视差突变。

---

8.7.3 StereoSGBM C++ 示例

cv::Mat grayL, grayR;

cv::cvtColor(rectL, grayL, cv::COLOR_BGR2GRAY);
cv::cvtColor(rectR, grayR, cv::COLOR_BGR2GRAY);

int minDisparity = 0;
int numDisparities = 128;  // 必须是 16 的倍数
int blockSize = 5;

cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
    minDisparity,
    numDisparities,
    blockSize
);

int channels = grayL.channels();

sgbm->setP1(8 * channels * blockSize * blockSize);
sgbm->setP2(32 * channels * blockSize * blockSize);
sgbm->setDisp12MaxDiff(1);
sgbm->setPreFilterCap(63);
sgbm->setUniquenessRatio(10);
sgbm->setSpeckleWindowSize(100);
sgbm->setSpeckleRange(32);
sgbm->setMode(cv::StereoSGBM::MODE_SGBM);

cv::Mat disparity16S;
sgbm->compute(grayL, grayR, disparity16S);

---

8.7.4 SGBM 参数说明

参数	含义	建议
minDisparity	最小视差	通常设为 0
numDisparities	视差搜索范围	必须是 16 的倍数
blockSize	匹配窗口大小	常用 3、5、7、9
P1	小视差变化惩罚	越大越平滑
P2	大视差变化惩罚	通常大于 P1
disp12MaxDiff	左右一致性检查阈值	常用 1
uniquenessRatio	唯一性约束	常用 5 到 15
speckleWindowSize	小连通域过滤窗口	常用 50 到 200
speckleRange	连通域内视差变化范围	常用 1 到 32
mode	SGBM 模式	MODE_SGBM 或 MODE_HH

---

8.8 视差后处理

直接由立体匹配得到的视差图通常存在噪声，需要进行后处理。

常见后处理方法包括：

方法	作用
左右一致性检查	剔除遮挡区域和误匹配点
中值滤波	去除孤立噪声点
双边滤波	平滑视差同时保留边缘
WLS 滤波	改善视差边缘和空洞
连通域过滤	删除小面积错误视差区域
空洞填充	填补无效视差区域
亚像素优化	提高视差精度

---

8.9 立体匹配总结

立体匹配的核心是：

对左图像中的每个像素
    ↓
在右图像对应极线上搜索候选点
    ↓
计算不同候选视差下的匹配代价或相似度
    ↓
选择最优视差
    ↓
生成视差图

对于不同匹配代价：

方法	最优视差选择方式
SAD	(d = \arg\min SAD)
SSD	(d = \arg\min SSD)
NCC	(d = \arg\max NCC)

其中，NCC 对亮度变化更加鲁棒，但计算量较大；SAD 和 SSD 计算简单，但对光照变化敏感；SGBM 在局部匹配代价基础上加入半全局约束，因此工程中更常用。

视差计算完成后，可以通过深度公式恢复三维信息：

$$Z=\frac{fB}{d}$$

9. 视差图处理

OpenCV 中 StereoSGBM 输出的视差图通常是 CV_16S 类型，并且真实视差被放大了 16 倍。

因此需要转换为浮点视差：

cv::Mat disparity;

disparity16S.convertTo(
    disparity,
    CV_32F,
    1.0 / 16.0
);

视差图可视化：

cv::Mat disparityVis;

cv::normalize(
    disparity,
    disparityVis,
    0,
    255,
    cv::NORM_MINMAX,
    CV_8U
);

cv::imshow("Disparity", disparityVis);
cv::imwrite("disparity.png", disparityVis);

注意：归一化后的 disparityVis 只是为了显示，不适合用于三维重建。三维重建应使用真实浮点视差 disparity。

---

10. 由视差恢复三维坐标

由深度公式：

$$Z=\frac{fB}{d}$$

可以恢复深度 (Z)。

然后根据针孔相机模型恢复 (X,Y)：

$$X=\frac{(u-c_x)Z}{f_x}$$

$$Y=\frac{(v-c_y)Z}{f_y}$$

因此：

像素坐标 (u, v) + 视差 d
        ↓
三维坐标 (X, Y, Z)

手动计算示例：

float fx = static_cast<float>(P1.at<double>(0, 0));
float fy = static_cast<float>(P1.at<double>(1, 1));
float cx = static_cast<float>(P1.at<double>(0, 2));
float cy = static_cast<float>(P1.at<double>(1, 2));

float B = static_cast<float>(std::abs(T.at<double>(0, 0)));

int u = 320;
int v = 240;

float d = disparity.at<float>(v, u);

if (d > 0.0f)
{
    float Z = fx * B / d;
    float X = (u - cx) * Z / fx;
    float Y = (v - cy) * Z / fy;

    std::cout << "X = " << X << std::endl;
    std::cout << "Y = " << Y << std::endl;
    std::cout << "Z = " << Z << std::endl;
}

需要注意的是：

三维坐标的单位取决于标定时棋盘格尺寸的单位。
如果棋盘格边长单位是 mm，则输出点云单位也是 mm；如果棋盘格边长单位是 m，则输出点云单位也是 m。

---

11. 使用 Q 矩阵恢复三维点云

OpenCV 中更常用的方式是使用 reprojectImageTo3D()：

cv::Mat points3D;

cv::reprojectImageTo3D(
    disparity,
    points3D,
    Q,
    true
);

其中：

参数	含义
disparity	浮点视差图
points3D	输出三维点图
Q	stereoRectify() 得到的重投影矩阵
true	是否处理缺失值

points3D 的尺寸与视差图相同，每个像素位置存储一个三维点：

cv::Vec3f point = points3D.at<cv::Vec3f>(v, u);

float X = point[0];
float Y = point[1];
float Z = point[2];

---

12. 保存点云为 PLY 文件

下面给出一个保存彩色点云的函数：

#include <opencv2/opencv.hpp>
#include <fstream>
#include <vector>
#include <cmath>

void savePointCloudPLY(
    const std::string& filename,
    const cv::Mat& points3D,
    const cv::Mat& colorImage,
    const cv::Mat& disparity
)
{
    std::ofstream ofs(filename);

    if (!ofs.is_open())
    {
        std::cerr << "无法打开文件: " << filename << std::endl;
        return;
    }

    std::vector<cv::Vec3f> points;
    std::vector<cv::Vec3b> colors;

    for (int y = 0; y < points3D.rows; y++)
    {
        for (int x = 0; x < points3D.cols; x++)
        {
            float d = disparity.at<float>(y, x);

            if (d <= 0.0f)
                continue;

            cv::Vec3f p = points3D.at<cv::Vec3f>(y, x);

            if (!std::isfinite(p[0]) ||
                !std::isfinite(p[1]) ||
                !std::isfinite(p[2]))
                continue;

            if (std::abs(p[2]) > 10000.0f)
                continue;

            points.push_back(p);
            colors.push_back(colorImage.at<cv::Vec3b>(y, x));
        }
    }

    ofs << "ply\n";
    ofs << "format ascii 1.0\n";
    ofs << "element vertex " << points.size() << "\n";
    ofs << "property float x\n";
    ofs << "property float y\n";
    ofs << "property float z\n";
    ofs << "property uchar red\n";
    ofs << "property uchar green\n";
    ofs << "property uchar blue\n";
    ofs << "end_header\n";

    for (size_t i = 0; i < points.size(); i++)
    {
        const cv::Vec3f& p = points[i];
        const cv::Vec3b& c = colors[i];

        ofs << p[0] << " "
            << p[1] << " "
            << p[2] << " "
            << static_cast<int>(c[2]) << " "
            << static_cast<int>(c[1]) << " "
            << static_cast<int>(c[0]) << "\n";
    }

    ofs.close();

    std::cout << "点云保存完成: " << filename << std::endl;
}

保存后的 .ply 文件可以使用 CloudCompare、MeshLab、PCL Viewer 等软件查看。

---

13. 完整 C++ 主流程代码

下面给出一个完整的双目重建示例。该示例假设已经完成双目标定，并将参数保存到 calibration.yml 中。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
#include <cmath>

using namespace cv;
using namespace std;

void savePointCloudPLY(
    const string& filename,
    const Mat& points3D,
    const Mat& colorImage,
    const Mat& disparity
)
{
    ofstream ofs(filename);

    if (!ofs.is_open())
    {
        cerr << "无法保存点云文件: " << filename << endl;
        return;
    }

    vector<Vec3f> points;
    vector<Vec3b> colors;

    for (int y = 0; y < points3D.rows; y++)
    {
        for (int x = 0; x < points3D.cols; x++)
        {
            float d = disparity.at<float>(y, x);

            if (d <= 0.0f)
                continue;

            Vec3f p = points3D.at<Vec3f>(y, x);

            if (!isfinite(p[0]) || !isfinite(p[1]) || !isfinite(p[2]))
                continue;

            if (fabs(p[2]) > 10000.0f)
                continue;

            points.push_back(p);
            colors.push_back(colorImage.at<Vec3b>(y, x));
        }
    }

    ofs << "ply\n";
    ofs << "format ascii 1.0\n";
    ofs << "element vertex " << points.size() << "\n";
    ofs << "property float x\n";
    ofs << "property float y\n";
    ofs << "property float z\n";
    ofs << "property uchar red\n";
    ofs << "property uchar green\n";
    ofs << "property uchar blue\n";
    ofs << "end_header\n";

    for (size_t i = 0; i < points.size(); i++)
    {
        Vec3f p = points[i];
        Vec3b c = colors[i];

        ofs << p[0] << " "
            << p[1] << " "
            << p[2] << " "
            << static_cast<int>(c[2]) << " "
            << static_cast<int>(c[1]) << " "
            << static_cast<int>(c[0]) << "\n";
    }

    ofs.close();

    cout << "点云保存完成: " << filename << endl;
}

int main()
{
    // 1. 读取左右图像
    Mat imgL = imread("left.png", IMREAD_COLOR);
    Mat imgR = imread("right.png", IMREAD_COLOR);

    if (imgL.empty() || imgR.empty())
    {
        cerr << "图像读取失败" << endl;
        return -1;
    }

    Size imageSize = imgL.size();

    // 2. 读取双目标定参数
    Mat K1, D1, K2, D2, R, T;

    FileStorage fs("calibration.yml", FileStorage::READ);

    if (!fs.isOpened())
    {
        cerr << "无法打开 calibration.yml" << endl;
        return -1;
    }

    fs["K1"] >> K1;
    fs["D1"] >> D1;
    fs["K2"] >> K2;
    fs["D2"] >> D2;
    fs["R"] >> R;
    fs["T"] >> T;

    fs.release();

    if (K1.empty() || D1.empty() || K2.empty() || D2.empty() || R.empty() || T.empty())
    {
        cerr << "标定参数不完整" << endl;
        return -1;
    }

    // 3. 双目极线校正
    Mat R1, R2, P1, P2, Q;
    Rect validROI1, validROI2;

    stereoRectify(
        K1,
        D1,
        K2,
        D2,
        imageSize,
        R,
        T,
        R1,
        R2,
        P1,
        P2,
        Q,
        CALIB_ZERO_DISPARITY,
        0,
        imageSize,
        &validROI1,
        &validROI2
    );

    // 4. 生成校正映射表
    Mat mapLx, mapLy;
    Mat mapRx, mapRy;

    initUndistortRectifyMap(
        K1,
        D1,
        R1,
        P1,
        imageSize,
        CV_32FC1,
        mapLx,
        mapLy
    );

    initUndistortRectifyMap(
        K2,
        D2,
        R2,
        P2,
        imageSize,
        CV_32FC1,
        mapRx,
        mapRy
    );

    // 5. 畸变校正 + 极线校正
    Mat rectL, rectR;

    remap(
        imgL,
        rectL,
        mapLx,
        mapLy,
        INTER_LINEAR
    );

    remap(
        imgR,
        rectR,
        mapRx,
        mapRy,
        INTER_LINEAR
    );

    imwrite("rectified_left.png", rectL);
    imwrite("rectified_right.png", rectR);

    // 6. 灰度化
    Mat grayL, grayR;

    cvtColor(rectL, grayL, COLOR_BGR2GRAY);
    cvtColor(rectR, grayR, COLOR_BGR2GRAY);

    // 7. SGBM 立体匹配
    int minDisparity = 0;
    int numDisparities = 128;  // 必须是 16 的倍数
    int blockSize = 5;

    Ptr<StereoSGBM> sgbm = StereoSGBM::create(
        minDisparity,
        numDisparities,
        blockSize
    );

    int channels = grayL.channels();

    sgbm->setP1(8 * channels * blockSize * blockSize);
    sgbm->setP2(32 * channels * blockSize * blockSize);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setPreFilterCap(63);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);
    sgbm->setMode(StereoSGBM::MODE_SGBM);

    Mat disparity16S;
    sgbm->compute(grayL, grayR, disparity16S);

    // 8. 视差转换
    Mat disparity;

    disparity16S.convertTo(
        disparity,
        CV_32F,
        1.0 / 16.0
    );

    // 9. 视差图可视化
    Mat disparityVis;

    normalize(
        disparity,
        disparityVis,
        0,
        255,
        NORM_MINMAX,
        CV_8U
    );

    imshow("Rectified Left", rectL);
    imshow("Rectified Right", rectR);
    imshow("Disparity", disparityVis);

    imwrite("disparity.png", disparityVis);

    // 10. 视差图恢复三维点云
    Mat points3D;

    reprojectImageTo3D(
        disparity,
        points3D,
        Q,
        true
    );

    // 11. 保存点云
    savePointCloudPLY(
        "point_cloud.ply",
        points3D,
        rectL,
        disparity
    );

    waitKey(0);

    return 0;
}

---

14. calibration.yml 示例

calibration.yml 中应保存左右相机内参、畸变参数以及双目外参。

%YAML:1.0

K1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ fx1, 0, cx1,
           0, fy1, cy1,
           0, 0, 1 ]

D1: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ k1, k2, p1, p2, k3 ]

K2: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ fx2, 0, cx2,
           0, fy2, cy2,
           0, 0, 1 ]

D2: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ k1, k2, p1, p2, k3 ]

R: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ r11, r12, r13,
           r21, r22, r23,
           r31, r32, r33 ]

T: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [ tx, ty, tz ]

需要注意：

• K1、D1 是左相机内参和畸变；
• K2、D2 是右相机内参和畸变；
• R、T 是双目标定得到的外参；
• T 的单位与标定板尺寸单位一致。

---

15. CMakeLists.txt 示例

cmake_minimum_required(VERSION 3.10)

project(StereoVisionDemo)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV REQUIRED)

add_executable(StereoVisionDemo main.cpp)

target_link_libraries(StereoVisionDemo ${OpenCV_LIBS})

编译运行：

mkdir build
cd build
cmake ..
make
./StereoVisionDemo

---

16. 深度误差分析

由深度公式：

$$Z=\frac{fB}{d}$$
假设焦距 (f) 和基线 (B) 已经标定准确，只考虑视差误差 (\Delta d)。

将 (Z) 看作关于视差 (d) 的函数：

$$Z(d)=\frac{fB}{d}$$

对 (d) 求导：

$$
\frac{\partial Z}{\partial d}

-\frac{fB}{d^2}
$$

根据一阶误差传播：

$$\Delta Z\approx \left|\frac{\partial Z}{\partial d}\right|\Delta d$$

因此：

$$\Delta Z\approx \frac{fB}{d^2}\Delta d$$

又因为：

$$Z=\frac{fB}{d}$$

所以：

$$d=\frac{fB}{Z}$$

代入上式：

$$\Delta Z\approx \frac{Z^2}{fB}\Delta d$$

最终得到：

$$\boxed{\Delta Z\approx \frac{Z^2}{fB}\Delta d}$$

可以得到深度误差近似关系：

$$\Delta Z\approx \frac{Z^2}{fB}\Delta d$$

其中：

参数	含义
(\Delta Z)	深度误差
(\Delta d)	视差误差
(Z)	目标距离
(f)	焦距
(B)	双目基线
该公式说明：

深度误差与距离的平方成正比，与焦距和基线成反比。

---

该公式说明：

1. 深度误差与距离平方成正比；
2. 物体越远，深度误差增长越快；
3. 焦距越大，深度误差越小；
4. 基线越大，深度误差越小；
5. 视差精度越高，深度精度越高。

双目视觉中，距离越远，视差越小，深度误差越大；并且深度误差会随着目标距离的平方快速增加。

因此，为了提高双目测量精度，可以从以下几个方面优化：

1. 提高相机分辨率；
2. 提高视差计算精度；
3. 合理增大双目基线；
4. 合理选择更大焦距的镜头；
5. 提高双目标定精度；
6. 使用亚像素级视差优化算法。

因此，双目系统设计时需要合理选择：

• 相机分辨率；
• 镜头焦距；
• 双目基线；
• 工作距离；
• 匹配算法；
• 标定精度。

---

17. 工程注意事项

17.1 标定精度决定系统上限

双目视觉中，标定误差会直接影响：

• 极线校正精度；
• 视差图质量；
• 深度计算精度；
• 点云尺度和形状。

如果标定不准，可能出现以下问题：

现象	原因
左右图同名点不在同一行	极线校正不准
视差图噪声大	匹配困难
点云弯曲	内参或畸变参数不准
测距偏差明显	基线或焦距误差

---

17.2 左右相机需要同步

对于静态场景，左右相机同步要求相对较低。

对于运动场景，左右相机不同步会导致严重误差。例如：

• 机器人运动；
• 车辆行驶；
• 传送带检测；
• 手持移动扫描；
• 动态人体或机械臂。

工程中建议使用：

• 硬件同步；
• 全局快门相机；
• 固定曝光；
• 固定增益；
• 固定白平衡。

---

17.3 合理设置视差范围

由：

$$d=\frac{fB}{Z}$$

可得：

$$d_{max}=\frac{fB}{Z_{min}}$$

$$d_{min}=\frac{fB}{Z_{max}}$$

其中：

参数	含义
(Z_{min})	最近测量距离
(Z_{max})	最远测量距离
(d_{max})	最大视差
(d_{min})	最小视差

numDisparities 应覆盖实际工作距离对应的视差范围。

如果视差范围太小，近处目标可能匹配不到。

如果视差范围太大，计算量增加，也更容易误匹配。

---

17.4 基线不是越大越好

增大基线 (B) 的优点：

• 视差增大；
• 深度精度提高；
• 远距离测量能力增强。

但是基线过大也会带来问题：

• 左右视角差异变大；
• 遮挡区域增多；
• 近距离目标可能无法同时被两个相机看到；
• 匹配难度增加。

因此基线需要根据工作距离和视场范围综合选择。

---

17.5 常见失败场景

场景	原因
白墙、纯色桌面	低纹理，缺少匹配特征
栅栏、网格、条纹	重复纹理，容易误匹配
玻璃、透明物体	成像不满足普通反射模型
镜面金属	左右图像反光不一致
物体边缘	遮挡导致左右图像不一致
远距离物体	视差太小，深度误差大
光照剧烈变化	灰度一致性假设失效

---

18. 双目视觉与结构光的关系

普通双目视觉属于被动视觉，依赖场景自身纹理进行匹配：

左相机 + 右相机

结构光属于主动视觉，通过投影仪主动给物体表面投射编码图案：

相机 + 投影仪

或者：

左相机 + 右相机 + 投影仪

结构光可以看作一种主动增强匹配信息的三维重建方式。

普通双目的主要问题是低纹理区域难以匹配，而结构光通过投影格雷码、相移条纹、多频条纹等图案，可以人为增加纹理或编码信息，从而提高匹配稳定性。

从几何角度看，投影仪也可以看成一个“反向相机”，相机和投影仪之间同样可以通过三角测量恢复三维坐标。

---

19. 总结

双目立体视觉的核心流程可以概括为：

1. 双目标定
   得到 K1, D1, K2, D2, R, T

2. 极线校正
   使用 stereoRectify 得到 R1, R2, P1, P2, Q

3. 图像重映射
   使用 initUndistortRectifyMap + remap 得到校正后的左右图像

4. 立体匹配
   使用 StereoSGBM 计算视差图

5. 视差转换
   将 OpenCV 输出的 16 倍定点视差转换为真实浮点视差

6. 三维重建
   使用 reprojectImageTo3D 或 Z = fB / d 计算三维点

7. 点云保存
   将三维点和颜色信息保存为 PLY 文件

双目视觉中最核心的公式是：

$$d=u_L-u_R$$

$$Z=\frac{fB}{d}$$

$$X=\frac{(u-c_x)Z}{f_x}$$

$$Y=\frac{(v-c_y)Z}{f_y}$$

其中最重要的是：

$$Z=\frac{fB}{d}$$

这说明双目视觉的深度精度本质上依赖于视差精度。

一句话总结：

双目立体视觉就是利用左右相机之间的基线形成三角测量，通过左右图像同名点的视差恢复物体的深度信息和三维坐标。