Hi,
I have created a ROS 2 driver for the OAK FFC 3P and 2 OAK FFC OV9282 cameras. I have synchronized the left and right raw images with Depthai C++API and after rectifying them, I am giving them the same timestamp in the ROS message header. The problem is that after publishing the rectified left and right images, the images received by SLAM, such as RTABMAP, are not synchronized, and there is also a time difference between consecutive frames when I run Isaac cuVSLAM. What am I doing wrong here? The callback timer and the DepthAI FPS are the same.
One strange thing I have noticed is that the time difference between the left and right images is 0.033 s at 30 fps and 0.016 s at 60 fps, according to RTABMAP. Why are they one frame apart? Also, the time difference between consecutive frames is a multiple of 0.066 seconds, according to Isaac CuVSLAM.
If you could give a solution or hint on what I am doing wrong, then I would really appreciate it.
The code is the following:
#include <cv_bridge/cv_bridge.hpp>
#include <depthai/depthai.hpp>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <json/json.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rmw/qos_profiles.h>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
class ImprovedStereoPublisher : public rclcpp::Node
{
public:
ImprovedStereoPublisher() : Node("improved_stereo_publisher")
{
this->get_logger().set_level(rclcpp::Logger::Level::Debug);
this->declare_parameter("sync_threshold_ms", 10);
int sync_threshold_ms = this->get_parameter("sync_threshold_ms").as_int();
// Create a custom QoS profile for SLAM stereo image publishers
rectifLeftImagePub =
this->create_publisher<sensor_msgs::msg::Image>("left/image_rect", qos);
rectifRightImagePub = this->create_publisher<sensor_msgs::msg::Image>(
"right/image_rect", qos);
leftCameraInfoPub = this->create_publisher<sensor_msgs::msg::CameraInfo>(
"left/camera_info", qos);
rightCameraInfoPub = this->create_publisher<sensor_msgs::msg::CameraInfo>(
"right/camera_info", qos);
load_calibration_data();
// Create and start the camera pipeline
run_camera_pipeline(sync_threshold_ms);
// Set up timer for periodic publishing
timer_ = this->create_wall_timer(
std::chrono::milliseconds(17), // ~60 FPS
std::bind(&ImprovedStereoPublisher::timer_callback, this));
RCLCPP_INFO(this->get_logger(), "ImprovedStereoPublisher initialized");
}
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rectifLeftImagePub,
rectifRightImagePub;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftCameraInfoPub,
rightCameraInfoPub;
cv::Mat cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, R1, R2,
P1, P2, Q;
cv::Mat map1_x, map1_y, map2_x, map2_y;
rclcpp::TimerBase::SharedPtr timer_;
// Pipeline components
std::unique_ptr<dai::Pipeline> pipeline;
std::shared_ptr<dai::Device> device;
std::shared_ptr<dai::node::MonoCamera> monoLeft, monoRight;
std::shared_ptr<dai::node::Sync> sync;
std::shared_ptr<dai::node::XLinkOut> xoutLeft, xoutRight;
std::shared_ptr<dai::DataOutputQueue> leftQueue, rightQueue;
// add demux node
// std::shared_ptr<dai::node::XLinkOut> xoutLeftDemux, xoutRightDemux;
// std::shared_ptr<dai::node::XLinkIn> xinLeft, xinRight;
std::shared_ptr<dai::node::MessageDemux> demux;
std::string get_calibration_file_path()
{
std::filesystem::path exec_path = std::filesystem::current_path();
std::filesystem::path json_file_path =
exec_path / "/home/shivam157/driver/src/depthai_ros_driver/src/"
"calibration_data.json";
return json_file_path.string();
}
void load_calibration_data()
{
std::string file_path = get_calibration_file_path();
std::ifstream file(file_path);
if (!file.is_open())
{
RCLCPP_ERROR(this->get_logger(),
"Failed to open calibration data file: %s",
file_path.c_str());
return;
}
Json::Value root;
Json::CharReaderBuilder reader;
std::string errs;
if (!Json::parseFromStream(reader, file, &root, &errs))
{
RCLCPP_ERROR(this->get_logger(), "Failed to parse calibration data: %s",
errs.c_str());
return;
}
try
{
cameraMatrix1 = cv::Mat(3, 3, CV_64F);
cameraMatrix2 = cv::Mat(3, 3, CV_64F);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
cameraMatrix1.at<double>(i, j) =
root["cameraMatrix1"][i][j].asDouble();
cameraMatrix2.at<double>(i, j) =
root["cameraMatrix2"][i][j].asDouble();
}
}
distCoeffs1 = cv::Mat(1, 5, CV_64F);
distCoeffs2 = cv::Mat(1, 5, CV_64F);
for (int i = 0; i < 5; ++i)
{
distCoeffs1.at<double>(0, i) = root["distCoeffs1"][0][i].asDouble();
distCoeffs2.at<double>(0, i) = root["distCoeffs2"][0][i].asDouble();
}
R = cv::Mat(3, 3, CV_64F);
T = cv::Mat(3, 1, CV_64F);
R1 = cv::Mat(3, 3, CV_64F);
R2 = cv::Mat(3, 3, CV_64F);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
R.at<double>(i, j) = root["R"][i][j].asDouble();
R1.at<double>(i, j) = root["R1"][i][j].asDouble();
R2.at<double>(i, j) = root["R2"][i][j].asDouble();
}
T.at<double>(i, 0) = root["T"][i][0].asDouble();
}
P1 = cv::Mat(3, 4, CV_64F);
P2 = cv::Mat(3, 4, CV_64F);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 4; ++j)
{
P1.at<double>(i, j) = root["P1"][i][j].asDouble();
P2.at<double>(i, j) = root["P2"][i][j].asDouble();
}
}
cv::Size imageSize(1280, 800);
cv::initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize,
CV_32FC1, map1_x, map1_y);
cv::initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imageSize,
CV_32FC1, map2_x, map2_y);
}
catch (const std::exception &e)
{
RCLCPP_ERROR(this->get_logger(), "Error loading calibration data: %s",
e.what());
}
}
void
updateCameraInfo(std::shared_ptr<sensor_msgs::msg::CameraInfo> &cameraInfo,
const cv::Mat &projectionMatrix, const cv::Size &imageSize)
{
cameraInfo->height = imageSize.height;
cameraInfo->width = imageSize.width;
cameraInfo->distortion_model = "plumb_bob";
cameraInfo->d = {0, 0, 0, 0, 0};
cameraInfo->k = {projectionMatrix.at<double>(0, 0),
0,
projectionMatrix.at<double>(0, 2),
0,
projectionMatrix.at<double>(1, 1),
projectionMatrix.at<double>(1, 2),
0,
0,
1};
cameraInfo->r = {1, 0, 0, 0, 1, 0, 0, 0, 1};
cameraInfo->p = {
projectionMatrix.at<double>(0, 0), projectionMatrix.at<double>(0, 1),
projectionMatrix.at<double>(0, 2), projectionMatrix.at<double>(0, 3),
projectionMatrix.at<double>(1, 0), projectionMatrix.at<double>(1, 1),
projectionMatrix.at<double>(1, 2), projectionMatrix.at<double>(1, 3),
projectionMatrix.at<double>(2, 0), projectionMatrix.at<double>(2, 1),
projectionMatrix.at<double>(2, 2), projectionMatrix.at<double>(2, 3)};
}
void run_camera_pipeline(int sync_threshold_ms)
{
auto pipeline = std::make_unique<dai::Pipeline>();
// Define sources and outputs
auto monoLeft = pipeline->create<dai::node::MonoCamera>();
auto monoRight = pipeline->create<dai::node::MonoCamera>();
auto sync = pipeline->create<dai::node::Sync>();
sync->setSyncThreshold(std::chrono::milliseconds(1));
auto demux = pipeline->create<dai::node::MessageDemux>();
auto xoutLeft = pipeline->create<dai::node::XLinkOut>();
auto xoutRight = pipeline->create<dai::node::XLinkOut>();
xoutLeft->setStreamName("left");
xoutRight->setStreamName("right");
// Properties
monoLeft->setResolution(
dai::MonoCameraProperties::SensorResolution::THE_800_P);
monoLeft->setCamera("left");
monoLeft->setFps(60.0);
monoRight->setResolution(
dai::MonoCameraProperties::SensorResolution::THE_800_P);
monoRight->setCamera("right");
monoRight->setFps(60.0);
// Linking
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 device and start pipeline
auto device =
std::make_shared<dai::Device>(*pipeline, dai::UsbSpeed::SUPER_PLUS);
auto leftQueue = device->getOutputQueue("left", 1, false);
auto rightQueue = device->getOutputQueue("right", 1, false);
// Store the pointers if needed
this->pipeline = std::move(pipeline);
this->device = device;
this->monoLeft = monoLeft;
this->monoRight = monoRight;
this->sync = sync;
this->xoutLeft = xoutLeft;
this->xoutRight = xoutRight;
this->demux = demux;
this->leftQueue = leftQueue;
this->rightQueue = rightQueue;
// // save one frame from each queue to see if initial frames are received
// auto leftFrame = leftQueue->get<dai::ImgFrame>();
// auto rightFrame = rightQueue->get<dai::ImgFrame>();
// // converting the frames to opencv mat
// cv::Mat leftImg = leftFrame->getCvFrame();
// cv::Mat rightImg = rightFrame->getCvFrame();
// // saving the frames
// cv::imwrite("left.jpg", leftImg);
// cv::imwrite("right.jpg", rightImg);
}
void timer_callback()
{
RCLCPP_INFO(this->get_logger(), "Timer callback triggered");
if (!leftQueue || !rightQueue)
{
RCLCPP_WARN(this->get_logger(), "Camera queues not initialized");
return;
}
auto leftFrame = leftQueue->get<dai::ImgFrame>();
auto rightFrame = rightQueue->get<dai::ImgFrame>();
if (leftFrame && rightFrame)
{
// printing the timestamp of the left and right frames in depthai format
// in seconds
// Convert DepthAI timestamps to seconds
double left_timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(
leftFrame->getTimestamp().time_since_epoch())
.count();
double right_timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(
rightFrame->getTimestamp().time_since_epoch())
.count();
// Log the timestamps
// RCLCPP_INFO(this->get_logger(), "Left frame timestamp: %f", left_timestamp);
// RCLCPP_INFO(this->get_logger(), "Right frame timestamp: %f", right_timestamp);
auto now = this->now();
cv::Mat leftImg = leftFrame->getCvFrame();
cv::Mat rightImg = rightFrame->getCvFrame();
cv::Mat rectifiedLeft, rectifiedRight;
cv::remap(leftImg, rectifiedLeft, map1_x, map1_y, cv::INTER_LINEAR);
cv::remap(rightImg, rectifiedRight, map2_x, map2_y, cv::INTER_LINEAR);
auto rectifiedLeftMsg =
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifiedLeft)
.toImageMsg();
auto rectifiedRightMsg =
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifiedRight)
.toImageMsg();
// Convert double timestamps to builtin_interfaces::msg::Time
builtin_interfaces::msg::Time left_ros_time;
left_ros_time.sec = static_cast<int32_t>(left_timestamp);
left_ros_time.nanosec = static_cast<uint32_t>((left_timestamp - left_ros_time.sec) * 1e9);
builtin_interfaces::msg::Time right_ros_time;
right_ros_time.sec = static_cast<int32_t>(right_timestamp);
right_ros_time.nanosec = static_cast<uint32_t>((right_timestamp - right_ros_time.sec) * 1e9);
// Assign the converted timestamps to the header.stamp fields
rectifiedLeftMsg->header.stamp = left_ros_time;
rectifiedRightMsg->header.stamp = right_ros_time;
rectifiedLeftMsg->header.frame_id = "left_camera_optical_frame";
rectifiedRightMsg->header.frame_id = "right_camera_optical_frame";
auto leftCameraInfo = std::make_shared<sensor_msgs::msg::CameraInfo>();
auto rightCameraInfo = std::make_shared<sensor_msgs::msg::CameraInfo>();
updateCameraInfo(leftCameraInfo, P1, rectifiedLeft.size());
updateCameraInfo(rightCameraInfo, P2, rectifiedRight.size());
leftCameraInfo->header = rectifiedLeftMsg->header;
rightCameraInfo->header = rectifiedRightMsg->header;
rectifLeftImagePub->publish(*rectifiedLeftMsg);
// print that the left image is published
RCLCPP_INFO(this->get_logger(), "Left image published");
rectifRightImagePub->publish(*rectifiedRightMsg);
// print that the right image is published
RCLCPP_INFO(this->get_logger(), "Right image published");
leftCameraInfoPub->publish(*leftCameraInfo);
rightCameraInfoPub->publish(*rightCameraInfo);
}
else
{
RCLCPP_WARN(this->get_logger(), "No frames received");
}
}
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ImprovedStereoPublisher>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
I am only creating my own ROS 2 driver because the official one didn't work, and I asked for help on github and in this forum, but no one helped.
