Struggling to align RGB and Depth images while maintaining high frame rate

Hi,

I have an Oak D Lite camera and am trying to perform SLAM + 3D spatial location estimation based on output from my YOLO object detection algorithm running on my robot's processor. I have the following requirements -

• Input to my object detection model must be a small RGB image for fast processing (640x480)
• Each point in the RGB image should match the Depth image (for spatial location estimation) and both images should be time-synchronized
• RGB and Depth images should be published at 30 FPS so I can use it for RGBD-SLAM

In my attempt to meet the above goals, I have written the script shared below. However, I don't think my RGB and Depth images are aligned when I just use color-camera Preview frames at 640x480 size (each point in my depth image does not correspond to the same point on my rgb image). And when I use stereo.DepthAlign(dai::CameraBoardSocket::CAM_A) to align depth and rgb, my depth image FPS is much lesser than 30 Hz (around 5-10Hz). How can I improve my script to achieve my requirements?

void OakDLite::camera_worker_(){
    // Create pipeline
    dai::Pipeline pipeline;

    // Define sources and outputs
    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
    auto monoRight = pipeline.create<dai::node::MonoCamera>();
    auto stereo = pipeline.create<dai::node::StereoDepth>();
    auto colorCam = pipeline.create<dai::node::ColorCamera>();
    auto sync = pipeline.create<dai::node::Sync>();
    auto imu = pipeline.create<dai::node::IMU>();

    auto xoutGrp = pipeline.create<dai::node::XLinkOut>();
    auto xoutIMU = pipeline.create<dai::node::XLinkOut>();


    // XLinkOut
    xoutGrp->setStreamName("xout");
    xoutIMU->setStreamName("imu");


    // Properties
    float fps = 30;
    monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_480_P);
    monoLeft->setFps(fps);
    monoLeft->setCamera("left");
    monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_480_P);
    monoRight->setFps(fps);
    monoRight->setCamera("right");

    // StereoDepth
    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_ACCURACY);
    stereo->setRectifyEdgeFillColor(0);  
    stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
    stereo->setLeftRightCheck(true);
    stereo->setExtendedDisparity(false);
    stereo->setSubpixel(true);

    // ColorCamera
    colorCam->setPreviewSize(640, 480);
    colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
    colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P);
    colorCam->setFps(fps);

    // IMU
    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 250);
    imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 250);
    imu->setBatchReportThreshold(1);
    imu->setMaxBatchReports(10);

    //Sync
    sync->setSyncThreshold(std::chrono::milliseconds(100));

    // StereoDepth Configuration
    auto config = stereo->initialConfig.get();
    config.postProcessing.speckleFilter.enable = true;
    config.postProcessing.speckleFilter.speckleRange = 10;
    config.postProcessing.temporalFilter.enable = false;
    config.postProcessing.spatialFilter.enable = false;
    config.postProcessing.brightnessFilter.maxBrightness = 250;
    config.postProcessing.decimationFilter.decimationFactor = 2; //4;
    config.postProcessing.decimationFilter.decimationMode = dai::RawStereoDepthConfig::PostProcessing::DecimationFilter::DecimationMode::NON_ZERO_MEDIAN;
    config.algorithmControl.depthUnit = dai::RawStereoDepthConfig::AlgorithmControl::DepthUnit::MILLIMETER;
    stereo->initialConfig.set(config);

    // Linking
    monoLeft->out.link(stereo->left);
    monoRight->out.link(stereo->right);

    stereo->depth.link(sync->inputs["depth"]);
    colorCam->preview.link(sync->inputs["rgb"]);
    stereo->syncedRight.link(sync->inputs["right"]);
    stereo->syncedLeft.link(sync->inputs["left"]);
    // stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
    sync->out.link(xoutGrp->input);
    imu->out.link(xoutIMU->input);

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

    auto device_infos = dai::Device::getAllAvailableDevices();
    std::shared_ptr<dai::Device> device;
    device = std::make_shared<dai::Device>(pipeline);

    auto queue = device->getOutputQueue("xout", 4, false);
    auto imuQueue = device->getOutputQueue("imu", 50, false);

    bool firstTs = false;
    auto baseTs = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>();

    while(!kill_thread_) {

        rclcpp::Time current_time = now();

        auto msgGrp = queue->tryGet<dai::MessageGroup>();
        if(msgGrp != nullptr){
            for(auto& frm : *msgGrp) {
                auto imgFrm = std::dynamic_pointer_cast<dai::ImgFrame>(frm.second);
                cv::Mat img = imgFrm->getCvFrame();


// More code below but omitted (gets the image and IMU frames and publishes them in ROS2).