How to display two images from a binocular camera synchronously with OpenCV - c++11

Question

I meet a problem that one of image from a binocular camera has a time delay corresponding to another image. I use ros package usb-cam to get the images with the time stamps and then use the OpenCV fuction imshow() to show them.
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.
Well, 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 with OpenCV functions.
You will see the codes in the following that I notes some codes.
The code are referred with the open resource project on the github. It is a C++ SLAM project about INS, GPS and binocular camera. I mainly refer the data capture in the rosNodeTest.cpp. It is about a multi-thread coding however I am not familiar with it.
Please visit https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
And there are my codes. It is a ros package and I am sure that it can run well. So there I don't paste the CMakeLists.txt and package.xml.
#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
// {std::cout<<"image1 is empty!"<<std::endl;}
// cv::waitKey();
//std::chrono::milliseconds dura(2);
//std::this_thread::sleep_for(dura);
}
}
int main(int argc, char** argv)
{
//Initialize a ros node
ros::init(argc,argv,"demo");
ros::NodeHandle n;
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
//Subscribe the binocular camera raw data
ros::Subscriber sub_img0=n.subscribe("/camera1/usb_cam1/image_raw", 2000, img0_callback);
ros::Subscriber sub_img1=n.subscribe("/camera2/usb_cam2/image_raw", 2000, img1_callback);
ROS_INFO("Wait for camera data.");
if(flag != 0) //for rviz
{
pubImg0 = n.advertise<sensor_msgs::Image>("/Img0", 100);
pubImg1 = n.advertise<sensor_msgs::Image>("/Img1", 100);
std::cout<<"for rviz"<<std::endl;
}
else //for cv
{
//Synchronization and display
std::cout<<"for cv"<<std::endl;
std::thread sync_thread{display};
sync_thread.detach();
//display();
}
ros::spin();
return 1;
}
The result now is that there is a camera slow. Is there some one to say something about the problem？ I am not sure that the problem is from the OpenCv display or multi-thread coding. Thank you!