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(); auto monoRight = pipeline.create(); auto stereo = pipeline.create(); // auto sync = pipeline.create(); // auto xoutGrp = pipeline.create(); auto xoutLeft = pipeline.create(); auto xoutRight = pipeline.create(); auto xoutDepth = pipeline.create(); // 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(); auto rightFrame = rightQueue->get(); 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(); auto rightCameraInfo = std::make_shared(); // 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); } } } } ```

Hi @"ShivamSharma"#p24386 You can use the Sync node, followed by Demux, to sync your streams. Example [here](https://docs.luxonis.com/software/depthai/examples/demux_message_group/). Thanks, Jaka

Hi @"jakaskerl"#p24410 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() ```

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

@"erik"#p24477 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?

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

@"erik"#p24526 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( "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.

Hi @"ShivamSharma"#2723 , We don't support RTABMAP ourselves, so I'd suggest using our implementation (that we support): https://github.com/luxonis/depthai-core/tree/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.

@"erik"#p24551 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?

@"ShivamSharma"#p24560 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.

@"erik"#p24566 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 from rerun_node import RerunNode File "/home/shivam157/depthai-core/examples/python/RVC2/VSLAM/rerun_node.py", line 13, in class RerunNode(dai.node.ThreadedHostNode): AttributeError: module 'depthai.node' has no attribute 'ThreadedHostNode' ``` What do I need to do to run this SLAM?

How to synchronize rectified left and right frames ?

michaelphlin

@erik Thanks for your help!!! the lib dependency is not longer a issue now.
After adding imu.enableFirmwareUpdate(True) line into rtabmap_vio_slam.py and re-running it, I receive the following error message:
Does that means I need to calibrate the IMU and camera again? or is there any factor calibrated matrix I can use for SLAM?
python3 rtabmap_vio_slam.py

[1844301041693CF500] [3.6] [1.682] [IMU(2)] [warning] IMU 'enableFirmwareUpdate' is deprecated! Use 'device.startIMUFirmwareUpdate'

[1844301041693CF500] [3.6] [1.710] [IMU(2)] [warning] Performing IMU firmware update, do NOT disconnect the device.

[1844301041693CF500] [3.6] [1.832] [IMU(2)] [warning] IMU firmware update status: 0%

[2024-07-30 15:59:45.717] [ThreadedNode] [error] Node threw exception, stopping the node. Exception message: IMU calibration data is not available on device yet.

erik

@michaelphlin are you using a device that has BNO085/6 imu?

michaelphlin

Hi @erik

Yes, based on the spec, I should have BNO086 on my OAK-D Pro and OAK-D Pro W.

If there’s extra step I need to initialize my IMU, please kindly let me know.

I really appreciate your help on this issue.

sincerely,

Michael

jakaskerl

Hi @michaelphlin
You can try adding the IMU extrinsics to the calibration of the device: luxonis/depthai-boardsblob/adding_IMU_tmp/boards/OAK-D-PRO.json

LMK if it solves the issue.

Thanks,
Jaka

ShivamSharma

@jakaskerl @erik Does depthai API or SDK have a script to calibrate IMU? How do I calculate the IMU extrinsics of the OAK FFC 3P? Will the slam work without IMU?

erik

Hi @ShivamSharma ,
We do not, extrinsics have to be extracted from CAD model (or measured). It needs to be spec-ed in the json file like this:
luxonis/depthai-boardsblob/main/boards/OAK-D-CM4-POE.json#L52-L72

ShivamSharma

erik So can I use the depthai slam without an imu?

erik

@ShivamSharma I don't think so, this is very much in WIP so we haven't done many tests/evaluations.

« Previous Page