Cam A on OAK-FFC-4P + OV 9782w is not synchronized.

DDaehyun · Oct 11, 2023

Hello,

I am currently testing with OAK FFC 4P(R7M1E7) with four OV 9782w cameras.

I implemented the ROS node(below code) based on the this link, but CAM A and CAM D are not synchronized.
(I am working on the develop branch of depthai-core with commit 57de7c0cafd5724e10adce3ca8f69c8230ac6e9c.)

#include <iostream>
#include <string>
#include <thread>

#include <opencv2/opencv.hpp>
#include <depthai/depthai.hpp>

#include <sensor_msgs/Imu.h>
#include <ros/ros.h>

#include "depthai_bridge/BridgePublisher.hpp"
#include "depthai_bridge/ImageConverter.hpp"
#include "depthai_bridge/ImuConverter.hpp"

int main(int argc, char** argv){
  ros::init(argc, argv, "dai_wrapper_node");
  ros::NodeHandle nh("~");
  dai::Pipeline pipeline;
  // pipeline.setXLinkChunkSize(0);

  auto control_in = pipeline.create<dai::node::XLinkIn>();
  auto color_a    = pipeline.create<dai::node::ColorCamera>();
  auto color_b    = pipeline.create<dai::node::ColorCamera>();
  auto color_c    = pipeline.create<dai::node::ColorCamera>();
  auto color_d    = pipeline.create<dai::node::ColorCamera>();
  
  auto imu        = pipeline.create<dai::node::IMU>();
  auto xout_imu   = pipeline.create<dai::node::XLinkOut>();

  control_in->setStreamName("control");
  control_in->out.link(color_a->inputControl);
  control_in->out.link(color_b->inputControl);
  control_in->out.link(color_c->inputControl);
  control_in->out.link(color_d->inputControl);

  xout_imu->setStreamName("imu");
  

  dai::node::ColorCamera::Properties::SensorResolution color_resolution;
  color_resolution = dai::node::ColorCamera::Properties::SensorResolution::THE_720_P;

  float color_fps = 1.0;
  //cameras 
  color_a->setResolution(color_resolution);
  color_a->setBoardSocket(dai::CameraBoardSocket::CAM_A);
  // color_a->setIspScale(1,2);
  color_a->initialControl.setFrameSyncMode(dai::CameraControl::FrameSyncMode::OUTPUT);
  color_a->initialControl.setSharpness(0);
  color_a->initialControl.setLumaDenoise(0);
  color_a->initialControl.setChromaDenoise(4);
  color_a->initialControl.setManualExposure(3000, 100);
  color_a->setFps(color_fps);

  color_b->setResolution(color_resolution);
  color_b->setBoardSocket(dai::CameraBoardSocket::CAM_B);

  color_b->initialControl.setFrameSyncMode(dai::CameraControl::FrameSyncMode::INPUT);
  color_b->initialControl.setSharpness(0);
  color_b->initialControl.setLumaDenoise(0);
  color_b->initialControl.setChromaDenoise(4);
  color_b->initialControl.setManualExposure(3000, 100);
  // color_b->setIspScale(1,2);
  color_b->setFps(color_fps);

  color_c->setResolution(color_resolution);
  color_c->setBoardSocket(dai::CameraBoardSocket::CAM_C);

  color_c->initialControl.setFrameSyncMode(dai::CameraControl::FrameSyncMode::INPUT);
  color_c->initialControl.setSharpness(0);
  color_c->initialControl.setLumaDenoise(0);
  color_c->initialControl.setChromaDenoise(4);
  color_c->initialControl.setManualExposure(3000, 100);
  color_c->setFps(color_fps);

  color_d->setResolution(color_resolution);
  color_d->setBoardSocket(dai::CameraBoardSocket::CAM_D);
  // color_d->setIspScale(1,2);
  color_d->initialControl.setFrameSyncMode(dai::CameraControl::FrameSyncMode::INPUT);
  color_d->initialControl.setSharpness(0);
  color_d->initialControl.setLumaDenoise(0);
  color_d->initialControl.setChromaDenoise(4);
  color_d->initialControl.setManualExposure(3000, 100);
  color_d->setFps(color_fps);

  //imu
  imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
  imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400);
  imu->setBatchReportThreshold(5);
  imu->setMaxBatchReports(20);

  auto xout_color_a = pipeline.create<dai::node::XLinkOut>();
  auto xout_color_b = pipeline.create<dai::node::XLinkOut>();
  auto xout_color_c = pipeline.create<dai::node::XLinkOut>();
  auto xout_color_d = pipeline.create<dai::node::XLinkOut>();

  xout_color_a->setStreamName("color_a");
  xout_color_b->setStreamName("color_b");
  xout_color_c->setStreamName("color_c");
  xout_color_d->setStreamName("color_d");

  color_a->isp.link(xout_color_a->input);
  color_b->isp.link(xout_color_b->input);
  color_c->isp.link(xout_color_c->input);
  color_d->isp.link(xout_color_d->input);
  

  imu->out.link(xout_imu->input);
  //-------------------------------------- connect device
  std::shared_ptr<dai::Device> device;
  std::vector<dai::DeviceInfo> available_devices = dai::Device::getAllAvailableDevices();

  dai::Device::Config cfg = pipeline.getDeviceConfig();
  cfg.board.gpio[6] = dai::BoardConfig::GPIO(dai::BoardConfig::GPIO::Direction::OUTPUT, dai::BoardConfig::GPIO::Level::HIGH);

  device = std::make_shared<dai::Device>(cfg, dai::UsbSpeed::SUPER_PLUS);
  // device = std::make_shared<dai::Device>(pipeline, dai::Device::getAllAvailableDevices().front(), dai::UsbSpeed::SUPER_PLUS);
  device->startPipeline(pipeline);


  // auto controlQueue = device->getInputQueue("control");

  //-------------------------------------- set outputs queue
  auto imu_queue = device->getOutputQueue("imu", 30, false);
  
  dai::ros::ImuSyncMethod imu_mode = static_cast<dai::ros::ImuSyncMethod>(1);
  //1-> LINEAR_INTERPOLATE_GYRO
  dai::rosBridge::ImuConverter imu_converter("oak_imu_frame", imu_mode, 0.0, 0.0);
  dai::rosBridge::ImageConverter converter("oak_a_camera_optical_frame", false);  
  

  dai::ros::ImageMsgs::CameraInfo cam_info;
  {
    cam_info.width = static_cast<uint32_t>(1280);
    cam_info.height = static_cast<uint32_t>(720);

    std::vector<double> flat_intrinsics;
    flat_intrinsics.assign(9, 0);

    std::copy(flat_intrinsics.begin(), flat_intrinsics.end() ,cam_info.K.begin());

    for(size_t i = 0 ; i < 8; i++){
      cam_info.D.push_back(static_cast<double>(0.0));
    }

    std::vector<double> stereo_flat_intrinsics, flat_rectified_rotation;
    stereo_flat_intrinsics.assign(12,0);
    flat_rectified_rotation.assign(9,0);

    std::copy(stereo_flat_intrinsics.begin(), stereo_flat_intrinsics.end(), cam_info.P.begin());
    std::copy(flat_rectified_rotation.begin(), flat_rectified_rotation.end(), cam_info.R.begin());

    cam_info.distortion_model = "rational_polynomial";
  }

  
  auto color_a_queue = device->getOutputQueue("color_a", 10, false);
  dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> color_a_pub(
    color_a_queue,
    nh,
    std::string("color_a"),
    std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &converter, std::placeholders::_1, std::placeholders::_2),
    30,
    cam_info,
    "color_a");

  color_a_pub.addPublisherCallback();

  auto color_b_queue = device->getOutputQueue("color_b", 10, false);
  dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> color_b_pub(
    color_b_queue,
    nh,
    std::string("color_b"),
    std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &converter, std::placeholders::_1, std::placeholders::_2),
    30,
    cam_info,
    "color_b");
  color_b_pub.addPublisherCallback();

  auto color_c_queue = device->getOutputQueue("color_c", 10, false);
  dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> color_c_pub(
    color_c_queue,
    nh,
    std::string("color_c"),
    std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &converter, std::placeholders::_1, std::placeholders::_2),
    30,
    cam_info,
    "color_c");
  color_c_pub.addPublisherCallback();
  
  auto color_d_queue = device->getOutputQueue("color_d", 10, false);
  dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> color_d_pub(
    color_d_queue,
    nh,
    std::string("color_d"),
    std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &converter, std::placeholders::_1, std::placeholders::_2),
    30,
    cam_info,
    "color_d");
  color_d_pub.addPublisherCallback();
  

  dai::rosBridge::BridgePublisher<sensor_msgs::Imu, dai::IMUData> imu_publish(
    imu_queue,
    nh,
    std::string("imu"),
    std::bind(&dai::rosBridge::ImuConverter::toRosMsg, &imu_converter, std::placeholders::_1, std::placeholders::_2),
    30,
    "",
    "imu");
  
  imu_publish.addPublisherCallback();

  ros::spin();
  return EXIT_SUCCESS;
}

Additionally, when I set CAM B to the sync OUTPUT configuration as described in this link, following error occur.
[14442C107183D5D600] [1.3.4.4] [2.120] [system] [critical] Fatal error. Please report to developers. Log: 'Fatal error on MSS CPU: trap: 00, address: 00000000' '0'

jakaskerl · Oct 11, 2023

cc @Luxonis-Adam

erik · Oct 11, 2023

Hi Daehyun ,
The GPIO 6 is actually incorrect, we apologize for the inconvenience. It was up to R5, but since R6 it has been changed to GPIO38 (more info here,, docs here).

DDaehyun · Oct 12, 2023

Hi @erik

I modified below line
cfg.board.gpio[6] = dai::BoardConfig::GPIO(dai::BoardConfig::GPIO::Direction::OUTPUT, dai::BoardConfig::GPIO::Level::HIGH);
to
cfg.board.gpio[38] = dai::BoardConfig::GPIO(dai::BoardConfig::GPIO::Direction::OUTPUT, dai::BoardConfig::GPIO::Level::HIGH);

but, It seems that CAM A and CAM D are still not synchronized.

please check the following image.

erik · Oct 12, 2023

Hi Daehyun ,
I just tried it myself, and it works as expected on the R5 with 2x OV9282 (CAM_D and CAM_C). Code can be found below. I also noticed that the 2lane and 4lane MIPI fsyncs are actually on by default on R7, so you don't need to set GPIO 38 on high.

DDaehyun · Oct 13, 2023

Hi erik

I've modified the code you provided to test the synchronization of four ov-9782-w cameras.
Yet, it appears they are still not perfectly synchronized. While the 'left' and 'right' cameras seem to be in sync, the 'rgb' and 'camd' cameras are not.

```

#!/usr/bin/env python3

import cv2
import math
import depthai as dai
import contextlib
import argparse
from datetime import timedelta

parser = argparse.ArgumentParser(epilog='Press C to capture a set of frames.')
parser.add_argument('-f', '--fps', type=float, default=4,
                    help='Camera sensor FPS, applied to all cams')

args = parser.parse_args()

cam_socket_opts = {
    'rgb'  : dai.CameraBoardSocket.RGB,
    'left' : dai.CameraBoardSocket.LEFT,
    'right': dai.CameraBoardSocket.RIGHT,
    'camd': dai.CameraBoardSocket.CAM_D,
}

def create_pipeline():
    # Start defining a pipeline
    pipeline = dai.Pipeline()
    def create(c, output=False):
        xout = pipeline.create(dai.node.XLinkOut)
        xout.setStreamName(c)

        cam = pipeline.create(dai.node.ColorCamera)
        cam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P)
        cam.isp.link(xout.input)
            # cam.initialControl.setManualExposure(1000, 800)
        if output:
            cam.initialControl.setFrameSyncMode(dai.CameraControl.FrameSyncMode.OUTPUT)
        else:
            cam.initialControl.setFrameSyncMode(dai.CameraControl.FrameSyncMode.INPUT)
        cam.setBoardSocket(cam_socket_opts[c])
        cam.setFps(args.fps)

    create('left')
    create('right')
    create('rgb', output=True)
    create('camd')
    return pipeline


# https://docs.python.org/3/library/contextlib.html#contextlib.ExitStack
with contextlib.ExitStack() as stack:
    device_infos = dai.Device.getAllAvailableDevices()

    if len(device_infos) == 0: raise RuntimeError("No devices found!")
    else: print("Found", len(device_infos), "devices")
    queues = []
    for device_info in device_infos:
    # for ip in ips:
    #     device_info = dai.DeviceInfo(ip)
        # Note: the pipeline isn't set here, as we don't know yet what device it is.
        # The extra arguments passed are required by the existing overload variants
        openvino_version = dai.OpenVINO.Version.VERSION_2022_1
        usb2_mode = False

        config = dai.Device.Config()
        # For FFC-4P R6 (revision 6) or higher, we changed FSIN_MODE_SELECT from GPIO6 to GPIO38!
        config.board.gpio[38] = dai.BoardConfig.GPIO(dai.BoardConfig.GPIO.OUTPUT,
                                                    dai.BoardConfig.GPIO.Level.HIGH)

        device = stack.enter_context(dai.Device(config=config))

        cam_list = {'left', 'right', 'rgb', 'camd'}
        # cam_list = {'left', 'right'}
        # cam_list = {'right', 'camd'}

        print('Starting pipeline for', device.getMxId())
        # Get a customized pipeline based on identified device type
        device.startPipeline(create_pipeline())

        # Output queue will be used to get the rgb frames from the output defined above
        for cam in cam_list:
            queues.append({
                'queue': device.getOutputQueue(name=cam, maxSize=4, blocking=False),
                'msgs': [], # Frame msgs
                'mx': device.getMxId(),
                'cam': cam
            })


    def check_sync(queues, timestamp):
        matching_frames = []
        for q in queues:
            for i, msg in enumerate(q['msgs']):
                time_diff = abs(msg.getTimestamp() - timestamp)
                # So below 17ms @ 30 FPS => frames are in sync
                if time_diff <= timedelta(milliseconds=math.ceil(500 / args.fps)):
                    matching_frames.append(i)
                    break

        if len(matching_frames) == len(queues):
            # We have all frames synced. Remove the excess ones
            for i, q in enumerate(queues):
                q['msgs'] = q['msgs'][matching_frames[i]:]
            return True
        else:
            return False

    while True:
        for q in queues:
            new_msg = q['queue'].tryGet()
            if new_msg is not None:
                print(f"[{q['mx']}] Got {q['cam']} frame, seq: {new_msg.getSequenceNum()} TS: {new_msg.getTimestamp()}")
                q['msgs'].append(new_msg)
                if check_sync(queues, new_msg.getTimestamp()):
                    for q in queues:
                        imgFrame: dai.ImgFrame = q['msgs'].pop(0)
                        frame = imgFrame.getCvFrame()
                        # If monochrome, convert to RGB
                        if frame.ndim == 2:
                            frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2RGB)
                        # Write text
                        frame = cv2.putText(frame, f"TS {imgFrame.getTimestamp(dai.CameraExposureOffset.MIDDLE)}", (10, 30), cv2.FONT_HERSHEY_TRIPLEX, 1.0, color=(127, 255, 0))
                        frame = cv2.putText(frame, f"Seq {imgFrame.getSequenceNum()}, Exposure: {imgFrame.getExposureTime().total_seconds() * 1000:.2f}ms", (10, 80), cv2.FONT_HERSHEY_TRIPLEX, 1.0, color=(127, 255, 0))
                        cv2.imshow(f"{q['cam']} - {q['mx']}", frame)
        if cv2.waitKey(1) == ord('q'):
            break

erik · Oct 13, 2023

I just tried with 4 of them, works as expected on the R5.. Note that you don't need to enable GPIO 38, as it's already connected - will update docs today.

DDaehyun · Oct 13, 2023

I've commented out the GPIO section in your code and executed it.

The images aren't displaying continuously; they seem to be frozen as attached image.

And, Could you please test it by reducing the fps to below 4? If you previously tested with a higher fps, it might be challenging to verify if the synchronization is accurate.

Thank you

erik · Oct 13, 2023

Daehyun It might be that there's something with the R7 as well, I believe they are being shipped to our office atm. We can test it by EOW next week, would that work for you?

DDaehyun · Oct 16, 2023

erik Yes, that works for me. Thank you for your service. I'll await your response

erik · Oct 25, 2023

(devices are currently being shipped to our office, will update once we receive them & test them)

DDaehyun · Oct 26, 2023

erik

Thank you!

I look forward to good news.

DDaehyun · Nov 9, 2023

@erik

I hope you haven't forgotten this

erik · Nov 9, 2023

Hi @Daehyun ,
Sorry about the delay. After quite some testing, we figured out the FSYNC signals were clashing, current workaround would be to set GPIO 42 to input;

    script = pipeline.create(dai.node.Script)
    script.setProcessor(dai.ProcessorType.LEON_CSS)
    script.setScript("""
        import GPIO
        GPIO.setup(42, GPIO.IN)
    """)

Full code below, which produces 4x synced frames:

DDaehyun · Nov 22, 2023

@erik
Thank you !

Now, It seems to be working well !