Cost Volume Initialization

Census Transform

The Census transform encodes local texture structure by comparing each neighbor pixel against the center pixel with in a window. This transform is robust to illumination variations since it only records relative intensity relationships.

void computeCensusTransform(const uint8* input, uint32* census_out, int img_height, int img_width) {
    constexpr int half_window = 2;
    constexpr int window_size = 5;
    
    for (int row = half_window; row < img_height - half_window; ++row) {
        for (int col = half_window; col < img_width - half_window; ++col) {
            const uint8 center_intensity = input[row * img_width + col];
            uint32 census_code = 0u;
            
            for (int dr = -half_window; dr <= half_window; ++dr) {
                for (int dc = -half_window; dc <= half_window; ++dc) {
                    census_code <<= 1;
                    const uint8 neighbor_intensity = input[(row + dr) * img_width + (col + dc)];
                    if (neighbor_intensity < center_intensity) {
                        census_code |= 1u;
                    }
                }
            }
            census_out[row * img_width + col] = census_code;
        }
    }
}

Hamming Distance Computation

Haming distance measures the bit-wise dissimilarity between two census codes. This operation is efficiently implemented using the Brian Kernighan's algorithm for counting set bits.

uint8 calculateHammingDistance32(uint32 a, uint32 b) {
    uint32 xor_result = a ^ b;
    uint8 bit_count = 0;
    
    while (xor_result) {
        ++bit_count;
        xor_result &= (xor_result - 1);
    }
    return bit_count;
}

uint8 calculateHammingDistance64(uint64 a, uint64 b) {
    uint64 xor_result = a ^ b;
    uint8 bit_count = 0;
    
    while (xor_result) {
        ++bit_count;
        xor_result &= (xor_result - 1);
    }
    return bit_count;
}

Cost Volume Construction

The initial cost volume stores matching costs for all pixel-disparity pairs. Memory layout follows row-major ordering: cost[d, y, x] maps to cost_array[y * width * D + x * D + d].

void buildInitialCostVolume(
    const uint8* left_img, const uint8* right_img,
    const uint32* left_census, const uint32* right_census,
    uint8* cost_volume,
    int height, int width,
    int min_disp, int max_disp) {
    
    const int disparity_range = max_disp - min_disp;
    
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            for (int d = min_disp; d < max_disp; ++d) {
                const int d_index = d - min_disp;
                const int ref_x = x - d;
                
                uint8 matching_cost = 0;
                if (ref_x >= 0 && ref_x < width) {
                    matching_cost = calculateHammingDistance32(
                        left_census[y * width + x],
                        right_census[y * width + ref_x]
                    );
                }
                cost_volume[y * width * disparity_range + x * disparity_range + d_index] = matching_cost;
            }
        }
    }
}

Cost Aggregation

Aggregation Strategy

SGM employs dynamic programming along multiple directions to propagate matching costs. Each path aggregation follows the principle: current pixel's cost equals its initial cost plus the minimum aggregated cost from the previous pixel along the path.

The implementation uses a rolling buffer strategy to maintain aggregated costs across scanlines:

void aggregateCostsAlongDirection(
    const uint8* initial_cost,
    uint8* aggregated_cost,
    int height, int width,
    int disp_range, int dir_x, int dir_y) {
    
    const int img_size = width * height * disp_range;
    std::fill(aggregated_cost, aggregated_cost + img_size, 0);
    
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            const int base_idx = (y * width + x) * disp_range;
            
            for (int d = 0; d < disp_range; ++d) {
                const int prev_x = x - dir_x;
                const int prev_y = y - dir_y;
                
                uint16 path_cost = initial_cost[base_idx + d];
                
                if (prev_x >= 0 && prev_x < width && prev_y >= 0 && prev_y < height) {
                    const int prev_idx = (prev_y * width + prev_x) * disp_range;
                    uint16 min_prev = UINT16_MAX;
                    
                    for (int pd = 0; pd < disp_range; ++pd) {
                        const uint16 penalty = (pd == d) ? 0 : P1;
                        const uint16 candidate = aggregated_cost[prev_idx + pd] + penalty;
                        min_prev = std::min(min_prev, candidate);
                    }
                    path_cost += min_prev;
                }
                aggregated_cost[base_idx + d] = path_cost;
            }
        }
    }
}

Multi-Path Aggregation

SGM aggregates costs along 4 or 8 directions. The final aggregated cost combines all directional pass results:

void combineAggregatedCosts(
    uint8* final_cost,
    const uint8* cost_1, const uint8* cost_2,
    const uint8* cost_3, const uint8* cost_4,
    const uint8* cost_5, const uint8* cost_6,
    const uint8* cost_7, const uint8* cost_8,
    int total_elements, int num_paths) {
    
    for (int i = 0; i < total_elements; ++i) {
        uint16 combined = cost_1[i] + cost_2[i] + cost_3[i] + cost_4[i];
        
        if (num_paths == 8) {
            combined += cost_5[i] + cost_6[i] + cost_7[i] + cost_8[i];
        }
        final_cost[i] = static_cast<uint8>(std::min(combined, 255));
    }
}

Disparity Selection and Refinement

Winner-Takes-All Selection

For each pixel, the disparity with minimum aggregated cost is selected as the optimal disparity:

void selectOptimalDisparity(
    const uint8* aggregated_cost,
    float32* disparity_map,
    int height, int width,
    int min_disp, int max_disp) {
    
    const int disp_range = max_disp - min_disp;
    
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            const int cost_offset = (y * width + x) * disp_range;
            
            uint16 min_cost = UINT16_MAX;
            int best_disp = min_disp;
            
            for (int d = min_disp; d < max_disp; ++d) {
                const uint16 cost_val = aggregated_cost[cost_offset + (d - min_disp)];
                if (cost_val < min_cost) {
                    min_cost = cost_val;
                    best_disp = d;
                }
            }
            disparity_map[y * width + x] = static_cast<float32>(best_disp);
        }
    }
}

Uniqueness Constraint

The uniqueness constraint filters unreliable disparity estimates by comparing the cost difference between the best and second-best disparity:

void enforceUniquenessConstraint(
    const uint8* cost_volume,
    float32* disparity_map,
    int height, int width,
    int min_disp, int max_disp,
    float32 uniqueness_threshold) {
    
    const int disp_range = max_disp - min_disp;
    
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            const int cost_offset = (y * width + x) * disp_range;
            
            uint16 best_cost = UINT16_MAX;
            uint16 second_best_cost = UINT16_MAX;
            int best_disp = min_disp;
            
            for (int d = min_disp; d < max_disp; ++d) {
                const uint16 cost_val = cost_volume[cost_offset + (d - min_disp)];
                if (cost_val < best_cost) {
                    second_best_cost = best_cost;
                    best_cost = cost_val;
                    best_disp = d;
                } else if (cost_val < second_best_cost) {
                    second_best_cost = cost_val;
                }
            }
            
            const float32 cost_ratio = static_cast<float32>(second_best_cost - best_cost) / best_cost;
            if (cost_ratio < (1.0f - uniqueness_threshold)) {
                disparity_map[y * width + x] = INVALID_DISPARITY;
            }
        }
    }
}

Sub-Pixel Refinement

Parabolic interpolation refines integer disparity to sub-pixel accuracy using the cost curve around the minimum:

float32 refineDisparitySubPixel(
    const uint8* cost_along_disp,
    int best_disparity,
    int min_disp, int max_disp) {
    
    if (best_disparity <= min_disp || best_disparity >= max_disp - 1) {
        return INVALID_DISPARITY;
    }
    
    const int d_minus = best_disparity - 1 - min_disp;
    const int d_plus = best_disparity + 1 - min_disp;
    
    const float32 c_minus = cost_along_disp[d_minus];
    const float32 c_0 = cost_along_disp[best_disparity - min_disp];
    const float32 c_plus = cost_along_disp[d_plus];
    
    const float32 denom = std::max(1.0f, c_minus + c_plus - 2.0f * c_0);
    const float32 sub_disp = static_cast<float32>(best_disparity) + 
                             (c_minus - c_plus) / (2.0f * denom);
    
    return sub_disp;
}

Left-Right Consistency Check

The L-R check detects occlusions and mismatches by verifying disparity consistency between stereo pairs:

void performLRConsistencyCheck(
    const float32* left_disp, const float32* right_disp,
    float32* filtered_disp,
    int height, int width,
    float32 threshold,
    std::vector<std::pair<int, int>>& occluded_pixels,
    std::vector<std::pair<int, int>>& mismatched_pixels) {
    
    occluded_pixels.clear();
    mismatched_pixels.clear();
    
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            const float32 disp_l = left_disp[y * width + x];
            
            if (disp_l == INVALID_DISPARITY) {
                mismatched_pixels.emplace_back(y, x);
                continue;
            }
            
            const int x_right = static_cast<int>(x - disp_l + 0.5f);
            
            if (x_right < 0 || x_right >= width) {
                filtered_disp[y * width + x] = INVALID_DISPARITY;
                mismatched_pixels.emplace_back(y, x);
                continue;
            }
            
            const float32 disp_r = right_disp[y * width + x_right];
            
            if (std::abs(disp_l - disp_r) > threshold) {
                const int x_rl = static_cast<int>(x_right + disp_r + 0.5f);
                
                if (x_rl > 0 && x_rl < width) {
                    const float32 disp_back = left_disp[y * width + x_rl];
                    if (disp_back > disp_l) {
                        occluded_pixels.emplace_back(y, x);
                    } else {
                        mismatched_pixels.emplace_back(y, x);
                    }
                } else {
                    mismatched_pixels.emplace_back(y, x);
                }
                filtered_disp[y * width + x] = INVALID_DISPARITY;
            } else {
                filtered_disp[y * width + x] = disp_l;
            }
        }
    }
}

Disparity Interpolation

Invalid disparities in occluded and mismatched regions are filled by interpolating from neighboring valid pixels:

void interpolateInvalidDisparities(
    float32* disparity_map,
    const std::vector<std::pair<int, int>>& invalid_locations,
    int height, int width,
    int max_search_radius) {
    
    constexpr float32 PI = 3.1415926f;
    const float32 directions[8] = { 
        0, PI/4, PI/2, 3*PI/4, PI, 5*PI/4, 3*PI/2, 7*PI/4 
    };
    
    for (const auto& pixel : invalid_locations) {
        const int y = pixel.first;
        const int x = pixel.second;
        
        std::vector<float32> collected_disparities;
        
        for (int dir = 0; dir < 8; ++dir) {
            const float32 angle = directions[dir];
            const float32 dy = std::sin(angle);
            const float32 dx = std::cos(angle);
            
            for (int step = 1; step < max_search_radius; ++step) {
                const int ny = static_cast<int>(std::lround(y + step * dy));
                const int nx = static_cast<int>(std::lround(x + step * dx));
                
                if (ny < 0 || ny >= height || nx < 0 || nx >= width) {
                    break;
                }
                
                const float32 neighbor_disp = disparity_map[ny * width + nx];
                if (neighbor_disp != INVALID_DISPARITY) {
                    collected_disparities.push_back(neighbor_disp);
                    break;
                }
            }
        }
        
        if (!collected_disparities.empty()) {
            std::sort(collected_disparities.begin(), collected_disparities.end());
            disparity_map[y * width + x] = collected_disparities[collected_disparities.size() / 2];
        }
    }
}

Implementation Notes

Sub-pixel refinement is essential for achieving sub-pixel accuracy in disparity estimation. The parabolic fitting technique exploits the smooth nature of the cost curve around the minimum.

Uniqueness constraint trades off computation time against accuracy. For GPU implementations where finding minima is computationally expensive, this step may be omitted at the cost of potential false matches.

Disparity filling inherently introduces uncertainty since occluded regions lack direct observation. The median selection strategy provides robustness against outliers, while selecting the second-smallest value for occlusions accounts for the fact that occluding pixels typically see the background rather than the true surface.