Depth and color not aligned

[cesarpgouveia]

Hi,

There appears to be a misalignment between the color and depth images produced by the OAK-D camera. Despite configuring the camera to align the depth map with the color image, the output images are not properly aligned. I have this C++ sample that initializes the OAK-D camera with both color and depth streams, attempts to align the depth stream with the color stream, and writes/shows both depth and color resized images for easier comparison and concatenates them side by side for visualization.

This is my C++ sample:

#include <iostream>
#include "depthai/depthai.hpp"

// Closer-in minimum depth, disparity range is doubled (from 95 to 190):
static std::atomic<bool> extended_disparity{ false };
// Better accuracy for longer distance, fractional disparity 32-levels:
static std::atomic<bool> subpixel{ false };
// Better handling for occlusions:
static std::atomic<bool> lr_check{ true };

// Custom parameters:
// Color camera sensor resolution
auto colorCameraResolution = dai::ColorCameraProperties::SensorResolution::THE_720_P;
// Color camera color order
auto colorCameraColorOrder = dai::ColorCameraProperties::ColorOrder::BGR;
// Depth camera sensor resolution
auto depthCameraResolution = dai::MonoCameraProperties::SensorResolution::THE_720_P;
// Depth stereo preset mode
auto depthPresetMode = dai::node::StereoDepth::PresetMode::HIGH_DENSITY;

// Width and height should match for color and depth to ensure alignment
const int frameWidth = 1280;
const int frameHeight = 720;

int main() {
    // Create pipeline
    dai::Pipeline pipeline;

    // Define sources and outputs

    // Initialize color camera
    auto camRgb = pipeline.create<dai::node::ColorCamera>();
    // Initialize depth camera
    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
    auto monoRight = pipeline.create<dai::node::MonoCamera>();
    auto depth = pipeline.create<dai::node::StereoDepth>();

    // Set two xout for two streams (color and depth)
    auto xoutColor = pipeline.create<dai::node::XLinkOut>();
    auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
    xoutColor->setStreamName("color");
    xoutDepth->setStreamName("depth");

    // Set color camera properties
    camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_A);
    camRgb->setResolution(colorCameraResolution);
    camRgb->setColorOrder(colorCameraColorOrder);
    camRgb->setInterleaved(false);
    camRgb->setPreviewSize(frameWidth, frameHeight);  // Ensuring it matches depth frame size
    camRgb->setVideoSize(frameWidth, frameHeight);    // Set video size for better alignment

    // Set mono camera properties
    monoLeft->setResolution(depthCameraResolution);
    monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
    monoRight->setResolution(depthCameraResolution);
    monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);

    // Set depth properties
    depth->setDefaultProfilePreset(depthPresetMode);
    depth->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
    depth->setLeftRightCheck(lr_check);
    depth->setExtendedDisparity(extended_disparity);
    depth->setSubpixel(subpixel);
    depth->setDepthAlign(dai::CameraBoardSocket::CAM_A);  // Enable depth alignment to color camera

    // Linking Color
    camRgb->video.link(xoutColor->input);  // Use video for aligned output
    // Linking Depth
    monoLeft->out.link(depth->left);
    monoRight->out.link(depth->right);
    depth->depth.link(xoutDepth->input);

    // Connect to device and start pipeline
    dai::Device device(pipeline);

    // Output queue color
    auto qColor = device.getOutputQueue("color");
    // Output queue depth
    auto qDepth = device.getOutputQueue("depth");

    int frameIdx = 0;
    while (true) {
        // Get color frame
        std::shared_ptr<dai::ImgFrame> inColor = qColor->get<dai::ImgFrame>();
        std::shared_ptr<dai::ImgFrame> inDepth = qDepth->get<dai::ImgFrame>();
        cv::Mat colorFrame = inColor->getCvFrame();
        cv::Mat depthFrame = inDepth->getFrame();

        // Normalize depth frame for visualization
        depthFrame.convertTo(depthFrame, CV_8UC1, 255.0 / depth->initialConfig.getMaxDisparity());

        // Apply color map for better visualization
        cv::Mat depthFrameColor;
        cv::applyColorMap(depthFrame, depthFrameColor, cv::COLORMAP_JET);

        // Display images
        cv::Mat colorFrameResized, depthFrameResized;

        // Resize images to 400x400
        cv::resize(colorFrame, colorFrameResized, cv::Size(800, 800));
        cv::resize(depthFrameColor, depthFrameResized, cv::Size(800, 800));

        // Concatenate images horizontally
        cv::Mat combinedImage;
        cv::hconcat(colorFrameResized, depthFrameResized, combinedImage);

        cv::imshow("Merge", combinedImage);

        std::string rootDir = "C:/Lixo/AlignOAKDTest/";
        cv::imwrite(rootDir + "merge_" + std::to_string(frameIdx) + ".jpg", combinedImage);
        frameIdx++;

        int key = cv::waitKey(1);
        if (key == 'q' || key == 'Q') {
            break;
        }
    }
    return 0;
}