How to synchronize rectified left and right frames ?

ShivamSharma

I have written my own ROS 2 driver using DepthAI C++ API. I create pipeline like the following:

// 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 sync = pipeline.create<dai::node::Sync>();

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

    auto xoutLeft = pipeline.create<dai::node::XLinkOut>();
    auto xoutRight = pipeline.create<dai::node::XLinkOut>();
    auto xoutDepth = pipeline.create<dai::node::XLinkOut>();

    // XLinkOut
    // xoutGrp->setStreamName("xout");
    xoutDepth->setStreamName("depth");
    xoutLeft->setStreamName("left");
    xoutRight->setStreamName("right");

    // Properties
    monoLeft->setResolution(
        dai::MonoCameraProperties::SensorResolution::THE_800_P);
    monoLeft->setCamera("left");
    monoLeft->setFps(30.0);
    monoRight->setResolution(
        dai::MonoCameraProperties::SensorResolution::THE_800_P);
    monoRight->setCamera("right");
    monoRight->setFps(30.0);

    stereo->setDefaultProfilePreset(
        dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
    stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
    // stereo->setInputResolution(1280, 720);
    stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5);
    stereo->setLeftRightCheck(lrcheck);
    stereo->setExtendedDisparity(extended);
    stereo->setSubpixel(subpixel);

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

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

    stereo->syncedLeft.link(xoutLeft->input);
    stereo->syncedRight.link(xoutRight->input);

    stereo->depth.link(xoutDepth->input);

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

    auto leftQueue = device.getOutputQueue("left", 8, false);
    auto rightQueue = device.getOutputQueue("right", 8, false);
    auto depthQueue = device.getOutputQueue("depth", 8, false);

My question is: how can I synchronize the raw frames from the pipeline or the rectified frames I am generating using OpenCV in my ROS2 driver?

I publish the frames like the following:

while (rclcpp::ok()) {
      auto leftFrame = leftQueue->get<dai::ImgFrame>();
      auto rightFrame = rightQueue->get<dai::ImgFrame>();

      if (leftFrame && rightFrame) {
        auto current_time = this->now();
        cv::Mat leftMat = leftFrame->getCvFrame();
        cv::Mat rightMat = rightFrame->getCvFrame();

        if (!leftMat.empty() && !rightMat.empty()) {
         
          cv::Mat rectifiedLeft, rectifiedRight;
          cv::remap(leftMat, rectifiedLeft, map1_x, map1_y, cv::INTER_LINEAR);
          cv::remap(rightMat, rectifiedRight, map2_x, map2_y, cv::INTER_LINEAR);

          auto rectifiedLeftImageMsg =
              cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
                                 rectifiedLeft)
                  .toImageMsg();

          rectifiedLeftImageMsg->header.frame_id = "left_camera_optical_frame";
          rectifiedLeftImageMsg->header.stamp = current_time;

          auto rectifiedRightImageMsg =
              cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
                                 rectifiedRight)
                  .toImageMsg();
          rectifiedRightImageMsg->header.frame_id =
              "right_camera_optical_frame";
          rectifiedRightImageMsg->header.stamp = current_time;

          // Updating the CameraInfo message
          auto leftCameraInfo =
              std::make_shared<sensor_msgs::msg::CameraInfo>();
          auto rightCameraInfo =
              std::make_shared<sensor_msgs::msg::CameraInfo>();

          // Fill in new intrinsic parameters derived from P1 and P2 (3x3
          // top-left submatrix)
          updateCameraInfo(leftCameraInfo, P1, rectifiedLeft.size());
          updateCameraInfo(rightCameraInfo, P2, rectifiedRight.size());

          leftCameraInfo->header.stamp = current_time;
          leftCameraInfo->header.frame_id = "left_camera_optical_frame";
          rightCameraInfo->header.stamp = current_time;
          rightCameraInfo->header.frame_id = "right_camera_optical_frame";

          leftCameraInfoPub->publish(*leftCameraInfo);
          rightCameraInfoPub->publish(*rightCameraInfo);
          rectifLeftImagePub->publish(*rectifiedLeftImageMsg);
          rectifRightImagePub->publish(*rectifiedRightImageMsg);

          
        }
      }
    }
  }

jakaskerl

Hi ShivamSharma
You can use the Sync node, followed by Demux, to sync your streams. Example here.

Thanks,
Jaka

ShivamSharma

Hi jakaskerl

How do I get an image in a suitable OpenCV format to show?

The following part of my code doesn't show an image:

with dai.Device(pipeline) as device:
    print("Start")
    q1 = device.getOutputQueue("left", maxSize=10, blocking=True)
    q2 = device.getOutputQueue("right", maxSize=10, blocking=True)
    while True:
        bufS1 = q1.get()
        bufS2 = q2.get()
        cv2.imshow("left", bufS1.getData())
        cv2.imshow("right", bufS2.getData())

Edit1:

I think I found a way to get synchronized frames. Does the following look correct to you for showing left and right synchronized frames:


import depthai as dai
import time
import numpy as np
import cv2
from datetime import timedelta

# Create a pipeline
pipeline = dai.Pipeline()

# Create mono camera nodes
monoLeft = pipeline.create(dai.node.MonoCamera)
monoRight = pipeline.create(dai.node.MonoCamera)

# Set properties for mono cameras
monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)
monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)

# Create XLinkOut nodes for individual camera streams (optional if only synchronization is required)
xoutLeft = pipeline.create(dai.node.XLinkOut)
xoutRight = pipeline.create(dai.node.XLinkOut)
xoutLeft.setStreamName("left")
xoutRight.setStreamName("right")

# Create sync node and set properties
sync = pipeline.create(dai.node.Sync)
# Set maximum allowed timestamp difference for synchronization
sync.setSyncThreshold(timedelta(milliseconds=100))
demux = pipeline.create(dai.node.MessageDemux)

# Link camera outputs to sync node
monoLeft.out.link(sync.inputs['left'])
monoRight.out.link(sync.inputs['right'])

sync.out.link(demux.input)
demux.outputs["left"].link(xoutLeft.input)
demux.outputs["right"].link(xoutRight.input)

# Connect to the device and start the pipelines
with dai.Device(pipeline) as device:
    print("Start")
    # Output queues will be used to get the frames from the outputs defined above
    q1 = device.getOutputQueue("left", maxSize=10, blocking=True)
    q2 = device.getOutputQueue("right", maxSize=10, blocking=True)
    while True:
        leftFrames = q1.get()  # Get synchronized frame object
        rightFrames = q2.get()

        # Get left and right frames from the synchronized object
        frameLeft = leftFrames.getCvFrame()
        frameRight = rightFrames.getCvFrame()

        # Display the frames
        cv2.imshow('Left Camera', frameLeft)
        cv2.imshow('Right Camera', frameRight)

        # Check for user input to close
        if cv2.waitKey(1) == ord('q'):
            break

    cv2.destroyAllWindows()

erik

Hi @ShivamSharma ,
Not sure why you are using demuxing. See example here:
https://docs.luxonis.com/software/depthai/examples/depth_video_sync/

ShivamSharma

erik I will be using the sync and demux nodes in the ROS 2 driver I am creating for my camera setup. If I don't use Demux, then I will have a message group, and I will have to add a for loop inside my while loop in the driver to get the left and right images.

Does using demux node affect the synchronization?

erik

@ShivamSharma it will work as well, up to the user. The code posted above looks correct, yes. Does it not work as expected?

ShivamSharma

erik
According to SLAM algorithms like RTABMAP and Isaac SLAM, it doesn't synchronize. Can you help me get good output from SLAM? I think the current issue is synchronization. I get jitter messages of 66 ms or 100 ms difference between consecutive frames when I run Isaac slam, and in RTABMAP I get a constant 0.033 s difference between the left and right frames. Rtabmap also complains sometimes that it has not received frames in the 5-second timeframe, even though the fps is around 25. I thought this was related to QoS policy in ROS, but I have tried different policies and have had no success with SLAM so far.

The QoS policy is the following:

// Create a custom QoS profile
    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
    custom_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
    custom_qos_profile.depth = 1; // Keep only the last message
    custom_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
    custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
    custom_qos_profile.deadline = RMW_QOS_DEADLINE_DEFAULT;
    custom_qos_profile.lifespan = RMW_QOS_LIFESPAN_DEFAULT;
    custom_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
    custom_qos_profile.liveliness_lease_duration =
        RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT;
    custom_qos_profile.avoid_ros_namespace_conventions = false;

    // Convert rmw_qos_profile_t to rclcpp::QoS
    auto qos =
        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos_profile));

    rectifLeftImagePub = this->create_publisher<sensor_msgs::msg::Image>(
        "left/image_rect", qos);

I process the synchronized raw frames with OpenCV using the rectification map created from the calibration, and then publish them next to each other inside the while loop. Where am I going wrong? I give the Depthai timestamp to the header of info and image messages.

Like the following:

rclcpp::Time frame_time(
            leftFrame->getTimestamp().time_since_epoch().count());

If you could help me with this issue, I would really appreciate it. My employment depends on getting the slam working on this OAK camera setup.

erik

Hi @ShivamSharma ,
We don't support RTABMAP ourselves, so I'd suggest using our implementation (that we support):
luxonis/depthai-coretree/v3_develop/examples/python/RVC2/VSLAM

WRT delays, 0.033 sec means left/right frames are just our of sync by 1 frame. I have no clue about QOS/ROS though, so can't help on that.

ShivamSharma

erik
Thanks a lot for your quick replies.

Will the slam that you mentioned run on the Jetson Orin Nano in real time with the custom oak camera setup I have?

Which branch of depthai_core I have to switch to? I currently use the main branch, which doesn't have a folder for slam. Will I be able to get the map in ROS 2 from this slam?

erik

ShivamSharma Yes, it will. Should even run on RPI5. Branch is in the URL, v3_develop. Not sure about the ros2 part, but you can just publish the map to ros I assume.

ShivamSharma

erik

Hi,

I changed the branch to the v3_develop branch. I installed it using the cmake command given in the readme file. When I first ran the slam file, I got an error for rerun and then I ran:
$ /usr/bin/python3 /home/shivam157/depthai-core/examples/python/install_requirements.py --install_rerun

Which gave me the following error:

ERROR: Failed building wheel for depthai
Failed to build depthai
ERROR: ERROR: Failed to build installable wheels for some pyproject.toml based projects (depthai)
Couldn't install dependencies as wheels and trying to compile from sources failed

Depthai-python was installed I don't know what the issue is.

After that, I ran:


$ python3 rtabmap_vio_slam.py 
Traceback (most recent call last):
  File "/home/shivam157/depthai-core/examples/python/RVC2/VSLAM/rtabmap_vio_slam.py", line 3, in <module>
    from rerun_node import RerunNode
  File "/home/shivam157/depthai-core/examples/python/RVC2/VSLAM/rerun_node.py", line 13, in <module>
    class RerunNode(dai.node.ThreadedHostNode):
AttributeError: module 'depthai.node' has no attribute 'ThreadedHostNode'

What do I need to do to run this SLAM?

erik

@ShivamSharma doesn't seem like it installed python depthai library correctly. I think wheels will be ready soon (so it won't try to build it locally and fail), see here:
luxonis/depthai-coreactions

ShivamSharma

erik
I have never used Github Actions, so it is not very clear to me how I can install Python Depthai. Can't I just use pip3 install depthai? Like given in this website

How long do I have to wait for wheels to be ready?

erik

Hi @ShivamSharma , it's just auto-building the wheels. Please retry install_requirements.py, it should work now.

ShivamSharma

erik

I ran Python3 install_requirements.py today and I still see that it is building the wheels and giving an error:

shivam157@ubuntu:~/depthai-core/examples/python$ python3 install_requirements.py
pip 24.2 from /home/shivam157/.local/lib/python3.10/site-packages/pip (python 3.10)
Requirement already satisfied: pip in /home/shivam157/.local/lib/python3.10/site-packages (24.2)
Requirement already satisfied: pyyaml in /home/shivam157/.local/lib/python3.10/site-packages (6.0.1)
Requirement already satisfied: requests in /home/shivam157/.local/lib/python3.10/site-packages (2.32.3)
Requirement already satisfied: charset-normalizer<4,>=2 in /home/shivam157/.local/lib/python3.10/site-packages (from requests) (2.0.12)
Requirement already satisfied: idna<4,>=2.5 in /usr/lib/python3/dist-packages (from requests) (3.3)
Requirement already satisfied: urllib3<3,>=1.21.1 in /usr/lib/python3/dist-packages (from requests) (1.26.5)
Requirement already satisfied: certifi>=2017.4.17 in /usr/lib/python3/dist-packages (from requests) (2020.6.20)
Looking in indexes: https://pypi.org/simple, https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local
Collecting depthai==3.0.0-alpha.0.dev0+25a9f0057d769b430bf3a543eaa8f33254518d03
  Using cached https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/depthai/depthai-3.0.0a0.dev0%2B25a9f0057d769b430bf3a543eaa8f33254518d03.tar.gz (187 kB)
  Installing build dependencies ... done
  Getting requirements to build wheel ... done
  Preparing metadata (pyproject.toml) ... done
Building wheels for collected packages: depthai
  Building wheel for depthai (pyproject.toml) ... error
  error: subprocess-exited-with-error
  
  × Building wheel for depthai (pyproject.toml) did not run successfully.
  \u2502 exit code: 1
  \u2570\u2500> [68 lines of output]
      /tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/dist.py:289: InformationOnly: Normalizing '3.0.0-alpha.0.dev0+25a9f0057d769b430bf3a543eaa8f33254518d03' to '3.0.0a0.dev0+25a9f0057d769b430bf3a543eaa8f33254518d03'
        self.metadata.version = self._normalize_version(self.metadata.version)
      running bdist_wheel
      running build
      running build_py
      creating build
      creating build/lib.linux-aarch64-cpython-310
      creating build/lib.linux-aarch64-cpython-310/depthai_cli
      copying depthai_cli/__init__.py -> build/lib.linux-aarch64-cpython-310/depthai_cli
      copying depthai_cli/depthai_cli.py -> build/lib.linux-aarch64-cpython-310/depthai_cli
      running build_ext
      CMake Warning:
        Ignoring extra path from command line:
      
         "/tmp"
      
      
      CMake Error: The source directory "/tmp" does not appear to contain CMakeLists.txt.
      Specify --help for usage, or press the help button on the CMake GUI.
      Traceback (most recent call last):
        File "/home/shivam157/.local/lib/python3.10/site-packages/pip/_vendor/pyproject_hooks/_in_process/_in_process.py", line 353, in <module>
          main()
        File "/home/shivam157/.local/lib/python3.10/site-packages/pip/_vendor/pyproject_hooks/_in_process/_in_process.py", line 335, in main
          json_out['return_val'] = hook(**hook_input['kwargs'])
        File "/home/shivam157/.local/lib/python3.10/site-packages/pip/_vendor/pyproject_hooks/_in_process/_in_process.py", line 251, in build_wheel
          return _build_backend().build_wheel(wheel_directory, config_settings,
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/build_meta.py", line 415, in build_wheel
          return self._build_with_temp_dir(
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/build_meta.py", line 397, in _build_with_temp_dir
          self.run_setup()
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/build_meta.py", line 497, in run_setup
          super().run_setup(setup_script=setup_script)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/build_meta.py", line 313, in run_setup
          exec(code, locals())
        File "<string>", line 220, in <module>
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/__init__.py", line 108, in setup
          return distutils.core.setup(**attrs)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/core.py", line 184, in setup
          return run_commands(dist)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/core.py", line 200, in run_commands
          dist.run_commands()
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/dist.py", line 970, in run_commands
          self.run_command(cmd)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/dist.py", line 945, in run_command
          super().run_command(command)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/dist.py", line 989, in run_command
          cmd_obj.run()
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/command/bdist_wheel.py", line 373, in run
          self.run_command("build")
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/cmd.py", line 316, in run_command
          self.distribution.run_command(command)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/dist.py", line 945, in run_command
          super().run_command(command)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/dist.py", line 989, in run_command
          cmd_obj.run()
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/command/build.py", line 135, in run
          self.run_command(cmd_name)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/cmd.py", line 316, in run_command
          self.distribution.run_command(command)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/dist.py", line 945, in run_command
          super().run_command(command)
        File "/tmp/pip-build-env-8tz847ck/overlay/local/lib/python3.10/dist-packages/setuptools/_distutils/dist.py", line 989, in run_command
          cmd_obj.run()
        File "<string>", line 93, in run
        File "<string>", line 217, in build_extension
        File "/usr/lib/python3.10/subprocess.py", line 369, in check_call
          raise CalledProcessError(retcode, cmd)
      subprocess.CalledProcessError: Command '['cmake', '/tmp', '-DDEPTHAI_BUILD_PYTHON=ON', '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=/tmp/pip-install-sjf68bin/depthai_306aec13e4264ef3b83fdb9d5d84ad54/build/lib.linux-aarch64-cpython-310/', '-DPYTHON_EXECUTABLE=/usr/bin/python3', '-DDEPTHAI_PYTHON_COMMIT_HASH=25a9f0057d769b430bf3a543eaa8f33254518d03', '-DCMAKE_BUILD_TYPE=Release', '-DHUNTER_CONFIGURATION_TYPES=Release', '-DHUNTER_JOBS_NUMBER=2']' returned non-zero exit status 1.
      [end of output]
  
  note: This error originates from a subprocess, and is likely not a problem with pip.
  ERROR: Failed building wheel for depthai
Failed to build depthai
ERROR: ERROR: Failed to build installable wheels for some pyproject.toml based projects (depthai)
Processing /home/shivam157/depthai-core/bindings/python
  Installing build dependencies ... done
  Getting requirements to build wheel ... done
  Preparing metadata (pyproject.toml) ... done
Building wheels for collected packages: depthai
  Building wheel for depthai (pyproject.toml) ... /


  note: This error originates from a subprocess, and is likely not a problem with pip.
  ERROR: Failed building wheel for depthai
Failed to build depthai
ERROR: ERROR: Failed to build installable wheels for some pyproject.toml based projects (depthai)
Couldn't install dependencies as wheels and trying to compile from sources failed

michaelphlin

Hi @erik , I have an OAK-D Pro and NVIDIA AGX Orin.
Can I run VIO with `dai.node.RTABMapVIO` in this setup? or does it only support RCV3?
and is there any documentation associated to VIO or VSLAM?
Thanks for you feedback in advance.

Michael

erik

michaelphlin Yep you can, in that case the RTABMapVIO would run on the NVIDIA AGX Orin, so device would only provide color/depth/IMU stream.

michaelphlin

ShivamSharma

@erik I have the same issue today after I follow the CMake instruction with v3_develop branch.

Is there any instruction can guide us step by step setup this VIO/ Rtabmap SLAM pipeline?

Thanks.

Michael

erik

@michaelphlin @ShivamSharma
Checkout to 4b6fb2bd on depthai-core. I used this commit as it has already built wheels. Go to examples/python, create venv, source it, call install_requirements.py. This will install required depthai libraries. Then go to RVC2/VSLAM folder, and run eg. basalt_vio.py. Note that this is heavily WIP, so some things won't work.