How to display two images from a binocular camera synchronously with OpenCV

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!