Stereo Depth Perception on ESP32 S3 | Baremetal

Settings

.xclk_freq_hz = CONFIG_XCLK_FREQ,
        .frame_size = FRAMESIZE_96X96,
        .pixel_format = PIXFORMAT_GRAYSCALE,
        .fb_location = CAMERA_FB_IN_DRAM,
        .fb_count = 1,
        .grab_mode = CAMERA_GRAB_WHEN_EMPTY

Disparity Calculation:

// Function to calculate binary descriptors
void compute_binary_descriptor(uint8_t* img, int width, int height, int x, int y, int block_size, uint64_t* descriptor) {
    int half_block = block_size / 2;
    int threshold = 128; // Threshold value for binary encoding

    *descriptor = 0;
    int bit_pos = 0;

    for (int by = -half_block; by <= half_block; by++) {
        for (int bx = -half_block; bx <= half_block; bx++) {
            int pixel_x = x + bx;
            int pixel_y = y + by;

            // Boundary check
            if (pixel_x >= 0 && pixel_x < width && pixel_y >= 0 && pixel_y < height) {
                int pixel = img[pixel_y * width + pixel_x];
                uint64_t bit = (pixel > threshold) ? 1 : 0;
                *descriptor |= (bit << bit_pos);
            } else {
                // If outside boundaries, treat as zero (or handle as needed)
                *descriptor |= (0 << bit_pos);
            }
            bit_pos++;
        }
    }
}

// Function to calculate Hamming distance between two descriptors
int hamming_distance(uint64_t d1, uint64_t d2) {
    uint64_t x = d1 ^ d2;
    int dist = 0;
    while (x) {
        dist += x & 1;
        x >>= 1;
    }
    return dist;
}

// Function to calculate disparity using block matching
void calculate_disparity_block_full_ORB(uint8_t* imgL, uint8_t* imgR, uint8_t* disparity, int width, int height, int max_disparity, int block_size) {
    // Initialize disparity map to zero
    memset(disparity, 0, width * height * sizeof(uint8_t));

    int half_block = block_size / 2;

    for (int y = half_block; y < height - half_block; y++) {
        for (int x = half_block; x < width - max_disparity - half_block; x++) {
            int min_hamming = INT_MAX;
            int best_disparity = 0;

            // Compute binary descriptor for the current block in the left image
            uint64_t left_descriptor;
            compute_binary_descriptor(imgL, width, height, x, y, block_size, &left_descriptor);

            // Compare blocks between left and right frames across full frame
            for (int d = 0; d < max_disparity; d++) {
                uint64_t right_descriptor;
                if (x - d - half_block >= 0) {
                    compute_binary_descriptor(imgR, width, height, x - d, y, block_size, &right_descriptor);

                    int hamming = hamming_distance(left_descriptor, right_descriptor);

                    if (hamming < min_hamming) {
                        min_hamming = hamming;
                        best_disparity = d;
                    }
                }
            }

            // Store the best disparity scaled to 255
            disparity[y * width + x] = (uint8_t)(best_disparity * 255 / max_disparity);
        }
    }
}