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

Daehyun

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'

erik

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).

jakaskerl

cc @Luxonis-Adam

Daehyun

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

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.

Daehyun

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

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.

Daehyun

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

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?

Daehyun

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

erik

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

Daehyun

erik

Thank you!

I look forward to good news.

Daehyun

@erik

I hope you haven't forgotten this 🙂

erik

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:

Daehyun

@erik
Thank you !

Now, It seems to be working well !