Time synchronization problem to show the images from a binocular camera [closed]
Backgrounds: Hi, everyone. I am a beginner of vision SLAM and now trying to show images in the real time of a binocular camera. Something I have done is that I have used the usb-cam
package to get the image data and shown them in the rviz
and rqt
. But I meet a time synchroniazation problem with the OpenCV imshow()
. Specificlly, I want to use usb-cam
to get the data and do some image procession with OpenCV
and at first I try to display the images.
Environment: Ubuntu 16.04 in the Vmware WorkStation 12 (in win10), ROS Kinetic, OpenCV 3.3.0. Binocular camera can support the mjepg format with 640*480 and 30fps.
Problem: I meet a problem that one of image from a binocular camera has a time delay corresponding to another image and the image is fixed that I mean that there is only that one camera with the time delay in the image display. Further, the time delay has a increase trend to be longer.
Codes :
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <queue>
#include <thread>
#include <mutex>
#include <iostream>
std::queue<sensor_msgs::ImageConstPtr> img0_buf;
std::queue<sensor_msgs::ImageConstPtr> img1_buf;
std::mutex m_buf;
//Test for rqt, 0 for cv, else for rviz
int flag = 0;
ros::Publisher pubImg0;
ros::Publisher pubImg1;
void img0_callback(const sensor_msgs::ImageConstPtr &img0)
{
m_buf.lock();
//For rqt
if(flag != 0)
{
sensor_msgs::Image img;
img.header=img0->header;
img.height = img0->height;
img.width = img0->width;
img.is_bigendian = img0->is_bigendian;
img.step = img0->step;
img.data=img0->data;
img.encoding=img0->encoding;
pubImg0.publish(img);
// std::cout<<"0."<<img.header<<std::endl;
}
else
{
img0_buf.push(img0);
}
m_buf.unlock();
}
void img1_callback(const sensor_msgs::ImageConstPtr &img1)
{
m_buf.lock();
if(flag != 0)
{
sensor_msgs::Image img;
img.header=img1->header;
img.height = img1->height;
img.width = img1->width;
img.is_bigendian = img1->is_bigendian;
img.step = img1->step;
img.data=img1->data;
img.encoding=img1->encoding;
pubImg1.publish(img);
// std::cout<<"1."<<img.header<<std::endl;
}
else
{
img1_buf.push(img1);
}
m_buf.unlock();
}
//Use the cv_bridge of ros to change the image data format from msgs to cv
cv::Mat msg2cv(const sensor_msgs::ImageConstPtr &img_msg)
{
cv_bridge::CvImageConstPtr ptr;
sensor_msgs::Image img_tmp;
img_tmp.header = img_msg->header;
img_tmp.height = img_msg->height;
img_tmp.width = img_msg->width;
img_tmp.is_bigendian = img_msg->is_bigendian;
img_tmp.step = img_msg->step;
img_tmp.data = img_msg->data;
img_tmp.encoding =img_msg->encoding;
ptr = cv_bridge::toCvCopy(img_tmp, sensor_msgs::image_encodings::BGR8);
cv::Mat img = ptr->image.clone();
return img;
}
//With reference of VINS rosNodeTest.cpp
void display()
{
while(1)
{
cv::Mat image0, image1;
// double t1,t2;
m_buf.lock();
if(!img0_buf.empty() && !img1_buf.empty())
{
ROS_INFO("Two cameras work");
image0=msg2cv(img0_buf.front());
// ROS_INFO("img0 %.9lf", img0_buf.front()->header.stamp.toSec());
// t1=img0_buf.front()->header.stamp.toSec();
img0_buf.pop();
imshow("camera1", image0);
image1=msg2cv(img1_buf.front());
// ROS_INFO("img1 %.9lf", img1_buf.front()->header.stamp.toSec());
// t2=img1_buf.front()->header.stamp.toSec();
img1_buf.pop();
cv::imshow("camera2", image1);
cv::waitKey(1);
}
m_buf.unlock();
// //display with cv
// if(!image0.empty())
// {
// imshow("camera1", image0);
// // cv::waitKey(1);
// }
// // else
// // { std::cout<<"image0 is empty!"<<std::endl;}
// if(!image1.empty())
// {
// imshow("camera2", image1);
// // cv::waitKey(1);
// }
// else ...