从0到1搭建一套属于你自己的高精度实时结构光3D相机（10）：基于绝对相位的立体匹配方法

1. 前言

在前面的博客中，介绍了二维误差扩散法下的1位深度正弦条纹生成算法。在这篇博客中，博主将介绍基于绝对相位的立体匹配方法。完成本篇博客的学习内容后，你将收获基于绝对相位的立体匹配方法与代码实践经验。

2. 传统立体匹配在结构光三维重建中的应用

博主不对立体匹配作详细描述，只在博文中通过较简洁的语言让你知道立体匹配的工作原理和一些经验。为什么不作详细描述呢？这回可真不是懒，因为有大神已经把这个讲解得非常明白了，博主可不想班门弄斧。

如果你想深刻了解一下立体匹配工作原理和过程，可以参考李迎松的立体匹配系列博文，在这里推荐各位跟着博文自己实现一遍完整的SGM算法。博主也跟着博文自己实现了完整的SGM算法，可以见博主Github仓库libStereoMatch。

我们来说一说传统立体匹配在结构光三维重建方面有什么不同与相同之处，我想各位有过复现立体匹配算法的同学，肯定会对这个有所疑惑。

我们先大致说一下立体匹配是什么。如图1所示[1]，立体匹配在左图像上逐像素查询该像素点与之对应的右图像像素点，然后以下式计算视差，从而形成视差图。

$$d=x_l-x_r$$

式中$x_l$是左图像像素点的横轴像素坐标，$x_r$是右图像像素点的横轴像素坐标，$d$为左图像像素点的视差值。

图1. 立体匹配过程

上述立体匹配过程一般可以分为以下几个步骤（代价计算、代价聚合、视差计算、视差优化）：

1）代价计算

代价计算的主要目的是衡量两像素点之间的相似度。常见的代价包含MI代价、AD代价、Census代价等。博主以AD代价作为说明，AD代价的计算公式为（以灰度图作说明）：

$$C_{ad}=abs(G_l-G_r)$$

式中，$C_{ad}$为AD代价值，$G_l$为左图像上像素点的灰度值，$G_r$为右图像上像素点的灰度值，符号$abs$表示取绝对值。

那么在基于绝对相位的立体匹配方法中，代价计算有什么相同点与不同点呢？

一般来说，由于我们的绝对相位是全局唯一的，此时AD代价能够完美发挥出它的优点。原因是，理论上左图像上某像素点的绝对相位值与右图像上某行逐像素像素值之差皆是唯一的。因此使用AD代价能够很好的找出对应点。

而Census代价则作用微乎其微，原因在于Census代价是一个判断某点与其临近点之间关系的代价方法。然而，我们是通过编码条纹的形式确定了这一关系，例如垂直条纹绝对相位都是从左至右递增的，所以，理论上，Census代价在任一像素点上都是相同的且为0（实际上即使不为0，也是非常小的值，不具有可区分度）。

结论就是，一般我们就用AD绝对代价即可！它能较为的完美发挥作用。

2）代价聚合

代价聚合的作用主要是将像素点与图像之间的关系加入进来，从而利用更多的信息对代价值进一步修正。根据代价聚合的方式，可以分为全局、半全局、局部立体匹配方法，顾名思义，这三者所作的代价聚合程度由高至低。

我们以SGM方法中的全局代价聚合公式来作为示例：

$$L(p,d)=C(p,d)+min\left\{\begin{array}{c}{L}_r(p-r,d)\\ L_r(p-r,d-1)+P_1\\ L_r(p-r,d+1)+P_1\\ \underset{i}{\min }{L}_r(p-r,i)+P_2\end{array}\right\}-\underset{i}{\min }{L}_r(p-r,i)$$

式中，符号$L(p,d)$代表聚合后的代价，$C(p,d)$代表当前像素视差为$d$时的代价值，$L_r(p-r,d)$代表该像素路径上一个像素视差为$d$时的代价，$L_r(p-r,d-1)$代表该像素路径上一个像素视差为$d-1$时的代价，$L_r(p-r,d+1)$代表该像素路径上一个像素视差为$d+1$时的代价，$undersetimin{L}_r(p-r,i)$代表该像素路径上一个像素所有视差下的最小代价值，$P_1$代表对视差较小（$=1$）的情况作惩罚的惩罚项，$P_2$代表对视差较大（$\ge 1$）的情况作惩罚的惩罚项。

貌似上面这一段得劝退很多人了，或许用图来说明更好点（摘自李迎松博客[2]），如图2所示：

图2. 全局代价聚合过程

其实就是将图像上所有的像素与当前点像素的信息加入进来，再对代价进一步修正，从而能够得到更精确的代价值。

那么在基于绝对相位的立体匹配方法中，代价聚合有什么相同点与不同点呢？

代价聚合可以说是传统立体匹配方法最最最核心的内容（当然在深度学习下的立体匹配方法中，也是最最最核心的内容）。这里说一下博主的思考与实践经验，博主认为代价聚合在基于绝对相位的立体匹配过程中可以不用，原因有以下几点：

• 由于绝对相位理论上是直线递增的，因此AD代价所计算的代价空间较为正确，噪声较小
• 基于绝对相位的立体匹配方法博主更倾向于认为它是一种局部立体匹配算法
• 实践中加入代价聚合步骤与不加入视差图结果差异不大

3）视差计算

视差计算的作用就是从上一步计算所得的代价空间中，选择代价值最小的视差值。这一步，基本上就是Winner Takes All（赢者通吃）算法了，也就是选择代价值最小的视差值。

那么在基于绝对相位的立体匹配方法中，视差计算有什么相同点与不同点呢？

博主认为，没有任何区别，实践亦是如此。

4）视差优化

视差优化是对上一步所计算得到的视差作进一步处理，例如通过亚像素拟合进一步提高视差图精度，通过左右一致性检验去除误匹配点，通过去除小连通域去除杂乱点，通过视差填充进一步处理视差缺失的区域，双边滤波等等。

那么在基于绝对相位的立体匹配方法中，视差优化有什么相同点与不同点呢？

博主认为，没有任何区别，实践亦是如此。值得一提的是，双边滤波能够让你的视差图看起来效果好很多（仅仅是看起来）。

3. 代码实践与实验

博主认为代码就不需要构建代价空间了，因为我们的代价是浮点型而且通常上结构光三维重建的图像都是数百兆甚至数千兆的，构造代价空间会导致运存过高，这对于低配硬件部署代码不太友好。

我们来看看博主写的代码（见博主仓库opencv_contrib下的sinus_comple_graycode_pattern.cpp）：

// TODO@Evans Liu: 增加水平条纹y轴方向支持
void SinusCompleGrayCodePattern_Impl::computeDisparity(
    InputArray leftUnwrapMap, InputArray rightUnwrapMap,
    OutputArray disparityMap) const {
    const Mat &leftUnwrap = *static_cast<const Mat *>(leftUnwrapMap.getObj());
    const Mat &rightUnwrap = *static_cast<const Mat *>(rightUnwrapMap.getObj());
    Mat &disparity = *static_cast<Mat *>(disparityMap.getObj());

    CV_Assert(!leftUnwrap.empty() && !rightUnwrap.empty());

    const int height = leftUnwrap.rows;
    const int width = leftUnwrap.cols;
    disparity = cv::Mat::zeros(height, width, CV_32FC1);

    parallel_for_(Range(0, height), [&](const Range &range) {
        for (int i = range.start; i < range.end; ++i) {
            auto leftUnwrapPtr = leftUnwrap.ptr<float>(i);
            auto rightUnwrapPtr = rightUnwrap.ptr<float>(i);
            auto disparityPtr = disparity.ptr<float>(i);
            
            for (int j = 0; j < width; ++j) {
                auto leftVal = leftUnwrapPtr[j];
				//跳过无效像素
                if (std::abs(leftVal) < 0.001f) {
                    continue;
                }

                float minCost = FLT_MAX;
                int bestDisp = 0;
				//计算代价并使用赢者通吃算法选择最优视差
                for (int d = params.minDisparity; d < params.maxDisparity;
                     ++d) {
                    if (j - d < 0 || j - d > width - 1) {
                        continue;
                    }

                    const float curCost =
                        std::abs(leftVal - rightUnwrapPtr[j - d]);

                    if (curCost < minCost) {
                        minCost = curCost;
                        bestDisp = d;
                    }
                }

                if (minCost < params.maxCost) {
                    if (bestDisp == params.minDisparity ||
                        bestDisp == params.maxDisparity) {
                        disparityPtr[j] = bestDisp;
                        continue;
                    }
					//亚像素插值
                    const float preCost =
                        std::abs(leftVal - rightUnwrapPtr[j - (bestDisp - 1)]);
                    const float nextCost =
                        std::abs(leftVal - rightUnwrapPtr[j - (bestDisp + 1)]);
                    const float denom =
                        std::max(1.f, preCost + nextCost - 2 * minCost);

                    disparityPtr[j] =
                        bestDisp + (preCost - nextCost) / (denom * 2.f);
                }
            }
        }
    });
}

是不是觉得很简单呢。博主认为自己写的CUDA加速代码相较CPU来说还是有点技术水平的，我们来看看吧，就暂时不写注释了，试试自己能看懂不。

const int ROW_PER_BLOCK = 8;
const int WARPS_PER_COL = 16;
const int THREADS_PER_GROUP = 32;
const dim3 BLOCK(THREADS_PER_GROUP, ROW_PER_BLOCK, 1);

struct CostDisp {
    __device__ CostDisp(const float &cost_, const int &disp_)
        : cost(cost_), disp(disp_) {}

    __device__ CostDisp() : cost(0.f), disp(0) {}

    float cost;
    int disp;
};

template <typename T, unsigned int GROUP_SIZE, unsigned int STEP>
struct subgroup_min_impl {
    static __device__ float2 call(const T &cost, const uint32_t &mask) {
        const T shflCost = __shfl_xor_sync(mask, cost, STEP / 2, GROUP_SIZE);
        const T minElement = shflCost > cost ? cost : shflCost;

        return subgroup_min_impl<T, GROUP_SIZE, STEP / 2>::call(minElement,
                                                                mask);
    }
};

template <typename T, unsigned int GROUP_SIZE>
struct subgroup_min_impl<T, GROUP_SIZE, 2u> {
    static __device__ float2 call(const T &cost, const uint32_t &mask) {
        float2 lastTwoMinElements;
        lastTwoMinElements.x = cost;
        lastTwoMinElements.y = __shfl_xor_sync(mask, cost, 1, GROUP_SIZE);

        return lastTwoMinElements;
    }
};

// never use it in current, because we apply different ratio in algorithm.
template <typename T, unsigned int GROUP_SIZE>
struct subgroup_min_impl<T, GROUP_SIZE, 1u> {
    static __device__ T call(const T &cost, const uint32_t &) { return cost; }
};

template <typename T, unsigned int GROUP_SIZE>
__device__ inline float2 subgroup_min(const T &cost, const uint32_t &mask) {
    return subgroup_min_impl<T, GROUP_SIZE, GROUP_SIZE>::call(cost, mask);
}

template <int DISP_PER_THREAD>
__global__ void stereo_match(const PtrStepSz<float> left,
                             const PtrStep<float> right, const int minDisp,
                             const int maxDisp, const float maxCost,
                             const float costMinDiff, PtrStep<float> dispMap) {
    const int dispBegin = threadIdx.x * DISP_PER_THREAD;
    // const int dispRange = maxDisp - minDisp;
    const int y = blockDim.y * blockIdx.y + threadIdx.y;
    const int pixelsProcess = left.cols / gridDim.x;
    const int xBegin = pixelsProcess * blockIdx.x;
    const int xEnd = xBegin + pixelsProcess;

    if (y > left.rows - 1)
        return;

    __shared__ float unwrapVal[ROW_PER_BLOCK][DISP_PER_THREAD]
                              [THREADS_PER_GROUP + 1];

    // init shared memory
#pragma unroll
    for (int d = 0; d < DISP_PER_THREAD; ++d) {
        const int rightX = xBegin - 1 - (minDisp + dispBegin + d);

        if (rightX < 0 || rightX > left.cols - 1) {
            unwrapVal[threadIdx.y][d][1 + threadIdx.x] = FLT_MAX;
        } else {
            unwrapVal[threadIdx.y][d][1 + threadIdx.x] =
                __ldg(&right.ptr(y)[rightX]);
        }
    }

    if (threadIdx.x == 0) {
        const int rightX = xBegin - 1 - (minDisp - 1);
        if (rightX < 0 || rightX > left.cols - 1) {
            unwrapVal[threadIdx.y][DISP_PER_THREAD - 1][0] = FLT_MAX;
        } else {
            unwrapVal[threadIdx.y][DISP_PER_THREAD - 1][0] =
                __ldg(&right.ptr(y)[rightX]);
        }
    }

    __syncthreads();

#pragma unroll
    for (int j = xBegin; j < xEnd; ++j) {
        // move to left
        const float rightLastVal =
            unwrapVal[threadIdx.y][DISP_PER_THREAD - 1][1 + threadIdx.x - 1];
        __syncthreads();
#pragma unroll
        for (int d = DISP_PER_THREAD - 1; d > 0; --d) {
            unwrapVal[threadIdx.y][d][1 + threadIdx.x] =
                unwrapVal[threadIdx.y][d - 1][1 + threadIdx.x];
        }

        unwrapVal[threadIdx.y][0][1 + threadIdx.x] = rightLastVal;

        if (threadIdx.x == 0) {
            const int rightX = j - (minDisp - 1);
            if (rightX < 0 || rightX > left.cols - 1) {
                unwrapVal[threadIdx.y][DISP_PER_THREAD - 1][0] = FLT_MAX;
            } else {
                unwrapVal[threadIdx.y][DISP_PER_THREAD - 1][0] =
                    __ldg(&right.ptr(y)[rightX]);
            }
        }

        __syncthreads();

        const float leftVal = __ldg(&left.ptr(y)[j]);

        if (abs(leftVal) < 0.001f) {
            dispMap.ptr(y)[j] = 0.f;
            continue;
        }

        CostDisp localBest(FLT_MAX, INT_MAX);
        float secondaryMinCost = FLT_MAX;

#pragma unroll
        for (int d = 0; d < DISP_PER_THREAD; ++d) {
            float cost =
                abs(leftVal - unwrapVal[threadIdx.y][d][1 + threadIdx.x]);

            if (localBest.cost > cost) {
                secondaryMinCost = localBest.cost;
                localBest.cost = cost;
                localBest.disp = d;
            }
        }
        __syncthreads();
        // find the minimum cost and best disparity
        const float2 lastTwoMinCosts =
            subgroup_min<float, 32>(localBest.cost, 0xffffffff);
        const float minCost = lastTwoMinCosts.x < lastTwoMinCosts.y
                                  ? lastTwoMinCosts.x
                                  : lastTwoMinCosts.y;
        const float diffCurPixel = abs(lastTwoMinCosts.x - lastTwoMinCosts.y);

        if (diffCurPixel < costMinDiff || minCost > maxCost)
            continue;

        if (abs(minCost - localBest.cost) < 0.001f &&
            abs(secondaryMinCost - minCost) > costMinDiff) {
            if ((threadIdx.x == 0 && localBest.disp == 0) ||
                (localBest.disp == DISP_PER_THREAD - 1 &&
                 threadIdx.x == blockDim.x - 1)) {
                dispMap.ptr(y)[j] = minDisp + dispBegin + localBest.disp;
            } else {
                float preCost, aftCost;
                if (localBest.disp == 0) {
                    preCost = abs(leftVal -
                                  unwrapVal[threadIdx.y][DISP_PER_THREAD - 1]
                                           [1 + threadIdx.x - 1]);
                    aftCost =
                        abs(leftVal - unwrapVal[threadIdx.y][localBest.disp + 1]
                                               [1 + threadIdx.x]);
                } else if (localBest.disp == DISP_PER_THREAD - 1) {
                    preCost =
                        abs(leftVal - unwrapVal[threadIdx.y][localBest.disp - 1]
                                               [1 + threadIdx.x]);
                    aftCost =
                        abs(leftVal -
                            unwrapVal[threadIdx.y][0][1 + threadIdx.x + 1]);
                } else {
                    preCost =
                        abs(leftVal - unwrapVal[threadIdx.y][localBest.disp - 1]
                                               [1 + threadIdx.x]);
                    aftCost =
                        abs(leftVal - unwrapVal[threadIdx.y][localBest.disp + 1]
                                               [1 + threadIdx.x]);
                }

                float denom = preCost + aftCost - 2.f * localBest.cost;
                denom = abs(denom) < 0.00001f ? 0.00001f : denom;
                dispMap.ptr(y)[j] = minDisp + dispBegin + localBest.disp +
                                    (preCost - aftCost) / (denom * 2.f);
            }
        }
    }
}

实验部分我们暂时先不放了，等下一节我们来个整体的。

4. 总结

在这篇博客中，博主介绍了基于绝对相位的立体匹配算法。博主貌似更新的有点快了，准备停更几天，等公众号关注人数上去了再更，看看有时间的话给大家讲讲别的小技巧。

参考文献

[1] Middlebury.Middlebury Stereo Datasets. [EB/OL]. 2024.3. https://vision.middlebury.edu/stereo/data/.

[2] 李迎松. 【理论恒叨】【立体匹配系列】经典SGM：（3）代价聚合（Cost Aggregation）. 2024.3. https://blog.csdn.net/rs_lys/article/details/83754473?spm=1001.2014.3001.5501.

本系列文章将持续更新，如果有等不及想要获得代码的小伙伴，可访问博主Github中的SLMaster项目，动动你的小手指，follow and star⭐！你们的关注是博主持续的动力！

Github：https://github.com/Practice3DVision

QQ群：229441078

公众号：实战3D视觉

扫一扫关注公众号