SGM重要代码摘抄

初始代价空间的形成

Census变换

void CensusTransform() const;

    // 逐像素计算census值
    for (sint32 i = 2; i < height - 2; i++) {
        for (sint32 j = 2; j < width - 2; j++) {
            
            // 中心像素值
            const uint8 gray_center = source[i * width + j];
            
            // 遍历大小为5x5的窗口内邻域像素，逐一比较像素值与中心像素值的的大小，计算census值
            uint32 census_val = 0u;
            for (sint32 r = -2; r <= 2; r++) {
                for (sint32 c = -2; c <= 2; c++) {
                    census_val <<= 1;
                    const uint8 gray = source[(i + r) * width + j + c];
                    if (gray < gray_center) {
                        census_val += 1;
                    }
                }
            }

            // 中心像素的census值
            census[i * width + j] = census_val;        
        }
    }

汉明距离

uint8 Hamming32(const uint32& x, const uint32& y);
uint8 Hamming64(const uint64& x, const uint64& y);

uint8 sgm_util::Hamming32(const uint32& x, const uint32& y)
{
    uint32 dist = 0, val = x ^ y;

    // Count the number of set bits
    while (val) {
        ++dist;
        val &= val - 1;
    }

    return static_cast<uint8>(dist);
}

结果

void ComputeCost() const;

代价空间形成程序：

auto& cost = cost_init_[i * width_ * disp_range + j * disp_range + (d - min_disparity)];

得到所有候选视差组成的初始代价空间，虽说的空间但实际上是一块长度为width*height*(max_disp-min_disp)的内存

代价聚合

思想：

总体思想利用动态规划：第一个像素代价为其初始视差，第二个像素代价为第二个像素初始代价和第一个像素的初始代价聚合得到聚合代价，第三个像聚合代价为第三个像素初始代价和第二个像素聚合代价的聚合...

技巧：

定义一个临时数组用来存储本像素的所有视差指下的聚合代价，给下一个像素用；

装入临时数组的技巧：

只需知道某一像素的最小视差的首地址，然后利用memcpy(&cost_last_path[1], cost_aggr_col, disp_range * sizeof(uint8));该函数表明以cost_aggr_col此次求得的聚合代价数组为起始地址拷贝disp_range * sizeof(uint8)长度的内存到以&cost_last_path[1]为起始地址的容器中

横向聚合：(参与聚合的元素个数为width个)

• 每个路径的起始为第一列的所有元素或最后一列的所有元素

1. 一级循环每次循环浏览第一列的某一行或最后一列的某一行
2. 二级循环每行从左向右或则从右向左遍历像素
3. 三级循环从最小视差到最大视差遍历

纵向聚合：(参与聚合的元素个数为height个)

• 每个路径的起始为第一行的所有元素或最后一行的所有元素

1. 一级循环每次循环浏览第一行的某一列或最后一行的某一列
2. 二级循环每行从上向下或则从下向上遍历像素
3. 三级循环从最小视差到最大视差遍历

对角聚合:(参与聚合的元素个数为height个)

• 每个路径的起始为第一行的所有元素或最后一行的所有元素

1. 一级循环每次循环浏览第一行的某一列或最后一行的某一列
2. 二级循环每行从左上向右下(从右上到左下)或则左下向右上(从右下到左上)
3. 三级循环从最小视差到最大视差遍历

结果：

得到所有候选视差组成的聚合代价空间，虽说的空间但实际上是一块长度为width*height*(max_disp-min_disp)的内存

总路径代价

    // 把4/8个方向加起来
    for (sint32 i = 0; i < size; i++) {
        if (option_.num_paths == 4 || option_.num_paths == 8) {
            cost_aggr_[i] = cost_aggr_1_[i] + cost_aggr_2_[i] + cost_aggr_3_[i] + cost_aggr_4_[i];
        }
        if (option_.num_paths == 8) {
            cost_aggr_[i] += cost_aggr_5_[i] + cost_aggr_6_[i] + cost_aggr_7_[i] + cost_aggr_8_[i];
        }
    }

最佳视差图的形成：

最佳视差图的形成：

求得某一像素的最佳视差后马上对它进行唯一性约束和子像素拟合，对每个像素都做以上操作可得到最佳视差图

最佳视差求解程序如下(就是对某像素所有视差下的代价“打擂台”，留下的是代价最小的视差)

            for (sint32 d = min_disparity; d < max_disparity; d++) {
                const sint32 d_idx = d - min_disparity;
                const auto& cost = cost_local[d_idx] = cost_ptr[i * width * disp_range + j * disp_range + d_idx];
                if(min_cost > cost) {
                    min_cost = cost;
                    best_disparity = d;
                }
            }

唯一性约束：(在确定最佳视差的函数中确定完最佳视差后实现)

//在确定最小代价所对应的视差同时确定次小代价所对应的视差用于唯一性约束
            if (is_check_unique) {
                // 再遍历一次，输出次最小代价值
                for (sint32 d = min_disparity; d < max_disparity; d++) {
                    if (d == best_disparity) {
                        // 跳过最小代价值
                        continue;
                    }
                    const auto& cost = cost_local[d - min_disparity];
                    sec_min_cost = std::min(sec_min_cost, cost);
                }

                // 判断唯一性约束
                // 若(min-sec)/min < min*(1-uniquness)，则为无效估计，uniqueness_ratio=0.95f
                if (sec_min_cost - min_cost <= static_cast<uint16>(min_cost * (1 - uniqueness_ratio))) {
                    disparity[i * width + j] = Invalid_Float;//视差图中无效视差
                    continue;//不满足条件最佳视差为无效值，然后跳过对该像素的处理
                }
            }

子像素拟合：(在确定最佳视差的函数中一致性检测实现后实现)

            // ---子像素拟合
            if (best_disparity == min_disparity || best_disparity == max_disparity - 1) {
                disparity[i * width + j] = Invalid_Float;//视差图中的无效视差
                continue;//不满足条件最佳视差为无效值，然后跳过对该像素的处理
            }
            // 最优视差前一个视差的代价值cost_1，后一个视差的代价值cost_2
            const sint32 idx_1 = best_disparity - 1 - min_disparity;
            const sint32 idx_2 = best_disparity + 1 - min_disparity;
            const uint16 cost_1 = cost_local[idx_1];
            const uint16 cost_2 = cost_local[idx_2];
            // 解一元二次曲线极值
            const uint16 denom = std::max(1, cost_1 + cost_2 - 2 * min_cost);
            disparity[i * width + j] = static_cast<float32>(best_disparity) + static_cast<float32>(cost_1 - cost_2) / (denom * 2.0f);//视差图中的有效视差

一致性检测(注意从此步开始因为上步的子像素拟合视差可能就不是整数)

void SemiGlobalMatching::LRCheck()

void SemiGlobalMatching::LRCheck()
{
    const sint32 width = width_;
    const sint32 height = height_;

    const float32& threshold = option_.lrcheck_thres;

    // 遮挡区像素和误匹配区像素
    auto& occlusions = occlusions_;
    auto& mismatches = mismatches_;
    occlusions.clear();
    mismatches.clear();

    // ---左右一致性检查
    for (sint32 i = 0; i < height; i++) {
        for (sint32 j = 0; j < width; j++) {
            // 左影像视差值
            auto& disp = disp_left_[i * width + j];
            if(disp == Invalid_Float){
                mismatches.emplace_back(i, j);
                continue;
            }

            // 根据视差值找到右影像上对应的同名像素
            const auto col_right = static_cast<sint32>(j - disp + 0.5);//加0.5是取最近的整数
            
            if(col_right >= 0 && col_right < width) {
                // 右影像上同名像素的视差值
                const auto& disp_r = disp_right_[i * width + col_right];
                
                // 判断两个视差值是否一致（差值在阈值内），不满足一致性检测的像素判断它是误匹配区的像素还是遮挡区的像素
                if (abs(disp - disp_r) > threshold) {
                    // 区分遮挡区和误匹配区
                    // 通过右影像视差算出在左影像的匹配像素，并获取视差disp_rl
                    const sint32 col_rl = static_cast<sint32>(col_right + disp_r + 0.5);
                    if(col_rl > 0 && col_rl < width){
                        const auto& disp_l = disp_left_[i*width + col_rl];
                        if(disp_l > disp) {
                            occlusions.emplace_back(i, j);
                        }
                        else {
                            mismatches.emplace_back(i, j);
                        }
                    }
                    else{
                        mismatches.emplace_back(i, j);//左像素在右图像的同名点根据同名点的视差在左图像上找不到同名点，则将左像素置为无效
                    }

                    // 让视差值无效
                    disp = Invalid_Float;
                }
            }
            else{
                // 通过视差值在右影像上找不到同名像素（超出影像范围）
                disp = Invalid_Float;
                mismatches.emplace_back(i, j);
            }
        }
    }

}

视差填充(浏览视差图所像素，将无效的像素进行视差填充)

void SemiGlobalMatching::FillHolesInDispMap()

void SemiGlobalMatching::FillHolesInDispMap()
{
    const sint32 width = width_;
    const sint32 height = height_;

    std::vector<float32> disp_collects;//将每个无效值的8个有效像素装入数组中（有效像素的寻找是重点)

    // 定义8个方向
    const float32 pi = 3.1415926f;
    float32 angle1[8] = { pi, 3 * pi / 4, pi / 2, pi / 4, 0, 7 * pi / 4, 3 * pi / 2, 5 * pi / 4 };
    float32 angle2[8] = { pi, 5 * pi / 4, 3 * pi / 2, 7 * pi / 4, 0, pi / 4, pi / 2, 3 * pi / 4 };
    float32 *angle = angle1;
    // 最大搜索行程，没有必要搜索过远的像素
    const sint32 max_search_length = 1.0*std::max(abs(option_.max_disparity), abs(option_.min_disparity));

    float32* disp_ptr = disp_left_;
    for (sint32 k = 0; k < 3; k++) {
        // 第一次循环处理遮挡区，第二次循环处理误匹配区，将想要处理的像素放在trg_pixels容器中，容器中元素类型：pair，代表行坐标和列坐标
        auto& trg_pixels = (k == 0) ? occlusions_ : mismatches_;
        if (trg_pixels.empty()) {
            continue;
        }
        std::vector<float32> fill_disps(trg_pixels.size());//trg_pixels.size()个0
        std::vector<std::pair<sint32, sint32>> inv_pixels;
        if (k == 2) {
            //  第三次循环处理前两次没有处理干净的像素
            for (sint32 i = 0; i < height; i++) {
                for (sint32 j = 0; j < width; j++) {
                    if (disp_ptr[i * width + j] == Invalid_Float) {
                        inv_pixels.emplace_back(i, j);
                    }
                }
            }
            trg_pixels = inv_pixels;
        }

        // 遍历待处理像素
        for (auto n = 0u; n < trg_pixels.size(); n++) {
            auto& pix = trg_pixels[n];
            //(i,j),i是行，纵向，高；j是列，横向，宽。图像分辨率是宽*高
            const sint32 y = pix.first;
            const sint32 x = pix.second;

            if (y == height / 2) {
                angle = angle2; 
            }

            // 收集8个方向上遇到的首个有效视差值
            disp_collects.clear();
            for (sint32 s = 0; s < 8; s++) {
                const float32 ang = angle[s];
                const float32 sina = float32(sin(ang));
                const float32 cosa = float32(cos(ang));
                for (sint32 m = 1; m < max_search_length; m++) {
                    const sint32 yy = lround(y + m * sina);//lround(*)返回最接近*的长整数
                    const sint32 xx = lround(x + m * cosa);
                    if (yy<0 || yy >= height || xx<0 || xx >= width) {
                        break;
                    }
                    const auto& disp = *(disp_ptr + yy*width + xx);
                    if (disp != Invalid_Float) {
                        disp_collects.push_back(disp);
                        break;
                    }
                }
            }
            if(disp_collects.empty()) {
                continue;
            }

            std::sort(disp_collects.begin(), disp_collects.end());

            // 如果是遮挡区，则选择第二小的视差值
            // 如果是误匹配区，则选择中值
            if (k == 0) {
                if (disp_collects.size() > 1) {
                    fill_disps[n] = disp_collects[1];
                }
                else {
                    fill_disps[n] = disp_collects[0];
                }
            }
            else{
                fill_disps[n] = disp_collects[disp_collects.size() / 2];
            }
        }
        for (auto n = 0u; n < trg_pixels.size(); n++) {
            auto& pix = trg_pixels[n];
            const sint32 y = pix.first;
            const sint32 x = pix.second;
            disp_ptr[y * width + x] = fill_disps[n];
        }
    }
}

注意

• 子像素拟合和一致性检查几乎是所有立体匹配算法必执行的策略。子像素拟合将整像素精度提高到子像素精度，而一致性检查可以说是剔除错误匹配的不二选择。唯一性约束和剔除小连通区可以视情况而添加，比如在GPU实现的时候，找最小值是时间复杂度较高的操作，而唯一性约束要找两次最小值（一个最小一个次最小）；而区域跟踪用GPU也难以高效的实现，所以这两块一般也可省掉。

• 但不得不讨论的是，视差填充始终是不精确的，无论是取最小值还是取中值，它只能说是通过周围的有效值来预测，所以精确程度是有限的，换句话说，遮挡区像素都看不见，何以预测出十分精确的值？所以我们通常会根据应用需求来决定是否执行视差填充，如果实际要求每个点足够准确，而不太要求是否足够完整，那么就不需要做视差填充；而如果要求视差图足够完整，而对填充精度要求不高，则可以执行视差填充。

参考文章:

https://ethanli.blog.csdn.net/article/details/106708643?spm=1001.2014.3001.5502