From b1a36200615b59493bdff4fc12760f294ff3a2b6 Mon Sep 17 00:00:00 2001
From: Davide Sapienza <sapienza.dav@gmail.com>
Date: Mon, 9 Sep 2019 14:40:43 +0200
Subject: [PATCH] Fix visualization thread

This commit splits some operations into different threads.
Some threads compute the visualization preprocessing for the
live, detection, top view and disparity visualization.
Only one thread has the role to display the different views.
---
 demo/demo/demo.cpp   | 714 ++++++++++++++++++++++++++++++++++---------
 include/classutils.h |   4 +-
 2 files changed, 570 insertions(+), 148 deletions(-)

diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp
index f9772f2..732c151 100644
--- a/demo/demo/demo.cpp
+++ b/demo/demo/demo.cpp
@@ -38,17 +38,32 @@
 bool gRun;
 std::string obj_class[10]{"person", "car", "truck", "bus", "motor", "bike", "rider", "traffic light", "traffic sign", "train"};
 
-std::mutex sem;
+// sem for mainthread, detectionthread and topviewthread
+// sem_vc for mainthread, videocapturethread, originalthread and disparitythread
+std::mutex sem, sem_vc;
+// a single mutex for each operation - the show_updates function must get all mutex
+std::mutex mutex_o, mutex_de, mutex_t, mutex_di;
 
-struct InfoShow{
+struct ModFrame_t{
     std::vector<Tracker> trackers;
     geodetic_converter::GeodeticConverter gc;
     double adfGeoTransform[6];
     cv::Mat H;
-    cv::Mat frame_v;
-    cv::Mat frame_disp;
+    cv::Mat original_frame;
+    tk::dnn::Yolo3Detection yolo;
+    cv::Mat mask;
 };
 
+struct Frame_t{
+    char *input;
+    cv::Mat frame;
+    int frame_nbr;
+};
+
+struct Show_t{
+    cv::Mat original, detection, topview, disparity;
+    bool update_o, update_de, update_t, update_di; 
+}updates;
 
 void sig_handler(int signo)
 {
@@ -56,25 +71,208 @@ void sig_handler(int signo)
     gRun = false;
 }
 
-void *showImages(void *x_void_ptr)
+void *readVideoCapture(void *x_void_ptr)
 {
-    InfoShow *info_show = (InfoShow *) x_void_ptr;
-    double lat, lon, alt;
-    int pix_x, pix_y;
-    cv::Mat frame_top;
-    cv::Mat original_frame_top;
+    Frame_t *info_f = (Frame_t *) x_void_ptr;
+    cv::VideoCapture cap(info_f->input);
+    cv::Mat frame_loc;
+    int frame_nbr_loc = 0;
+    // bool to_show = false;
+    if (!cap.isOpened())
+        gRun = false;
+    else
+        std::cout << "camera started\n";
+    
+
+    cap.set(cv::CAP_PROP_BUFFERSIZE,3);
+    // cap.set(CV_CAP_PROP_FPS, 10);
+    // // CV_CAP_PROP_BUFFERSIZE
+    std::cout<<"buf size: "<<cap.get(CV_CAP_PROP_BUFFERSIZE)<<std::endl;
+
+    // ModFrame_t *info_show = (ModFrame_t *) x_void_ptr;
+    auto start_t = std::chrono::steady_clock::now();
+    auto step_t = std::chrono::steady_clock::now();
+    auto end_t = std::chrono::steady_clock::now();
+
+    // compute fps
+    // std::cout << "Frames per second using video.get(CV_CAP_PROP_FPS) : " << cap.get(CV_CAP_PROP_FPS) << std::endl;
+
+    // //compute frame rate 
+    // int i=0, num_f = 120;
+    // while (i<21)
+    // {        
+    //     cap >> frame;
+    //     i++;
+    // }
+
+    // i=0;
+    // start_t = std::chrono::steady_clock::now();
+    // while (i<num_f)
+    // {        
+    //     step_t = std::chrono::steady_clock::now();
+
+    //     cap >> frame;
+    //     i++;
+    //     std::cout << " step "<<i<<" : "<<std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - step_t).count() << " ms"<<std::endl;
+        
+    // }
+    // end_t = std::chrono::steady_clock::now();    
+
+    // std::cout << "Capturing " << num_f << " frames" << std::endl ;
+    // std::cout << " Time taken : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
+   
+    // return 0;
+    //------------------------
+
+    while (gRun)
+    {        
+        cap >> frame_loc;
+        // CAP_PROP_POS_MSEC Current position of the video file in milliseconds or video capture timestamp.
+        std::cout<<"id: "<<cap.get(cv::CAP_PROP_POS_MSEC)<<std::endl;
+        // CAP_PROP_FRAME_COUNT Number of frames in the video file.
+        // std::cout<<"id: "<<cap.get(cv::CAP_PROP_FRAME_COUNT)<<std::endl;
+        
+        if (!frame_loc.data)
+        {
+            usleep(1000000);
+            cap.open(info_f->input);
+            printf("cap reinitialize\n");
+            continue;
+        }
+
+        end_t = std::chrono::steady_clock::now();
+        std::cout << " VC-TIME 1 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
+        start_t = end_t;
+
+        sem_vc.lock();
+        info_f->frame = frame_loc.clone();
+        info_f->frame_nbr = frame_nbr_loc;
+        sem_vc.unlock();
+        end_t = std::chrono::steady_clock::now();
+        std::cout << " VC-TIME 2 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
+        start_t = end_t;
+        frame_nbr_loc ++;
+    }
+}
+
+/* Thread function to show the updated images 
+**/
+void *show_updates(void *x_void_ptr)
+{
+    cv::namedWindow("original", cv::WINDOW_NORMAL);
     cv::namedWindow("detection", cv::WINDOW_NORMAL);
     cv::namedWindow("topview", cv::WINDOW_NORMAL);
     cv::namedWindow("disparity", cv::WINDOW_NORMAL);
+    cv::Mat original_loc, detection_loc, topview_loc, disparity_loc;
+    bool update_o_loc, update_de_loc, update_t_loc, update_di_loc;
+    
+    while (gRun)
+    {
+        TIMER_START
+        // critical section: copy the struct in local variable
+        // in this way we can unlock the sem for the main thread
+        if(mutex_o.try_lock())
+            {
+            update_o_loc = updates.update_o;
+            updates.update_o = false;
+            if(update_o_loc)
+                original_loc = updates.original.clone();
+            mutex_o.unlock();
+            }
 
-    // original_frame_top = cv::imread("../demo/demo/data/map/map_geo.jpg");
-    original_frame_top = cv::imread("../demo/demo/data/map/MASA_4670.png");
-    cv::Mat frame_v_loc;
+        if(mutex_de.try_lock())
+            {    
+            update_de_loc = updates.update_de;
+            updates.update_de = false;
+            if(update_de_loc)
+                detection_loc = updates.detection.clone();
+            mutex_de.unlock();
+            }
+
+        if(mutex_t.try_lock())
+            {  
+            update_t_loc = updates.update_t;
+            updates.update_t = false;
+            if(update_t_loc)
+                topview_loc = updates.topview.clone();
+            mutex_t.unlock();
+            }
+
+        if(mutex_di.try_lock())
+            {  
+            update_di_loc = updates.update_di;
+            updates.update_di = false;
+            if(update_di_loc)
+                disparity_loc = updates.disparity.clone();
+            mutex_di.unlock();
+            }
+
+        if(update_o_loc)
+            cv::imshow("original", original_loc);
+        if(update_de_loc)
+            cv::imshow("detection", detection_loc);
+        if(update_t_loc)
+            cv::imshow("topview", topview_loc);
+        if(update_di_loc)
+            cv::imshow("disparity", disparity_loc);
+        cv::waitKey(1);
+        // usleep(20000);  //sleep 20 msec
+        std::cout<<"show_updates: ";
+        TIMER_STOP
+    }
+}
+
+void *originalFrame(void *x_void_ptr)
+{
+    Frame_t *info_show_orig = (Frame_t *) x_void_ptr;
+    cv::Mat frame_loc;
+    int frame_nbr_loc = 0;
+    while (gRun)
+    {
+        TIMER_START
+        // critical section: copy the struct in local variable
+        // in this way we can unlock the sem for the main thread
+        sem_vc.lock();
+        frame_loc = info_show_orig->frame.clone();
+        frame_nbr_loc = info_show_orig->frame_nbr;
+        sem_vc.unlock();
+        if (frame_nbr_loc == 0)
+        {
+            usleep(1000000);
+            printf("no frame received\n");
+            continue;
+        }
+        mutex_o.lock();
+        updates.original = frame_loc.clone();
+        updates.update_o = true;
+        mutex_o.unlock();
+        usleep(10000);  //sleep 10 msec
+        std::cout<<"originalFrame: ";
+        TIMER_STOP
+    }
+}
+
+void *detectionFrame(void *x_void_ptr)
+{
+    ModFrame_t *info_show = (ModFrame_t *) x_void_ptr;
+    double lat, lon, alt;
+    int pix_x, pix_y;
+    cv::Mat original_frame_loc;
     std::vector<Tracker> trackers;
     geodetic_converter::GeodeticConverter gc;
     double adfGeoTransform[6];
     cv::Mat H;
-    cv::Mat frame_disp_loc;
+    tk::dnn::Yolo3Detection yolo;
+    int num_detected;
+    cv::Mat mask;
+
+    // box variable
+    tk::dnn::box b;
+    int x0, w, x1, y0, h, y1;
+    int objClass;
+    std::string det_class;;
+    float prob; 
+    cv::Scalar intensity;
 
     while (gRun)
     {
@@ -82,7 +280,7 @@ void *showImages(void *x_void_ptr)
         // critical section: copy the struct in local variable
         // in this way we can unlock the sem for the main thread
         sem.lock();
-        frame_v_loc = info_show->frame_v.clone();
+        original_frame_loc = info_show->original_frame.clone();
         // std::vector<Tracker> trackers;
         trackers = info_show->trackers;
         // geodetic_converter::GeodeticConverter gc;
@@ -91,10 +289,111 @@ void *showImages(void *x_void_ptr)
             adfGeoTransform[i] = info_show->adfGeoTransform[i];
         // cv::Mat H;
         H = info_show->H.clone();
-        frame_disp_loc = info_show->frame_disp.clone();
-        
+        yolo = info_show->yolo;
+        mask = info_show->mask.clone();
         sem.unlock();
         
+        if (trackers.empty())
+        {
+            usleep(1000000);
+            printf("no data available\n");
+            continue;
+        }
+
+        num_detected = yolo.detected.size();
+        for (int i = 0; i < num_detected; i++)
+        {
+            b = yolo.detected[i];
+            x0 = b.x;
+            w = b.w;
+            x1 = b.x + w;
+            y0 = b.y;
+            h = b.h;
+            y1 = b.y + h;
+            objClass = b.cl;
+            det_class = obj_class[b.cl];
+            prob = b.prob;
+
+            intensity = mask.at<uchar>(cv::Point(int(x0 + b.w / 2), y1));
+            
+            if (intensity[0] && objClass < 6)
+            {
+                //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
+                cv::rectangle(original_frame_loc, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2);
+                // draw label
+                int baseline = 0;
+                float fontScale = 0.5;
+                int thickness = 2;
+                cv::Size textSize = getTextSize(det_class, cv::FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
+                cv::rectangle(original_frame_loc, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), yolo.colors[b.cl], -1);
+                cv::putText(original_frame_loc, det_class, cv::Point(x0, (y0 - (baseline / 2))), cv::FONT_HERSHEY_SIMPLEX, fontScale, cv::Scalar(255, 255, 255), thickness); 
+            }
+        }
+
+        for (auto t : trackers)
+        {
+            for (size_t p = 1; p < t.pred_list_.size(); p++)
+            {
+                gc.enu2Geodetic(t.pred_list_[p].x_, t.pred_list_[p].y_, 0, &lat, &lon, &alt);
+                
+                coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
+
+                std::vector<cv::Point2f> map_p, camera_p;
+                map_p.push_back(cv::Point2f(pix_x, pix_y));
+
+                //transform camera pixel to map pixel
+                cv::perspectiveTransform(map_p, camera_p, H.inv());
+                cv::circle(original_frame_loc, cv::Point(camera_p[0].x, camera_p[0].y), 3.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0);
+            }
+        }
+
+        mutex_de.lock();
+        updates.detection = original_frame_loc.clone();
+        updates.update_de = true;
+        mutex_de.unlock();
+
+        std::cout<<"detectionFrame: ";
+        TIMER_STOP
+
+    }
+}
+
+void *topviewFrame(void *x_void_ptr)
+{
+    ModFrame_t *info_show = (ModFrame_t *) x_void_ptr;
+    double lat, lon, alt;
+    int pix_x, pix_y;
+    cv::Mat frame_top;
+    cv::Mat original_frame_top;
+    // original_frame_top = cv::imread("../demo/demo/data/map/map_geo.jpg");
+    // original_frame_top = cv::imread("../demo/demo/data/map/MASA_4670.png");
+    original_frame_top = cv::imread("../demo/demo/data/map/MASA_4670_V.png");
+    
+    std::vector<Tracker> trackers;
+    geodetic_converter::GeodeticConverter gc;
+    double adfGeoTransform[6];
+    cv::Mat H;
+    while (gRun)
+    {
+        TIMER_START
+        // critical section: copy the struct in local variable
+        // in this way we can unlock the sem for the main thread
+        sem.lock();
+        // std::vector<Tracker> trackers;
+        trackers = info_show->trackers;
+        // geodetic_converter::GeodeticConverter gc;
+        gc = info_show->gc;
+        for(int i = 0; i < 6; i++ )
+            adfGeoTransform[i] = info_show->adfGeoTransform[i];
+        // cv::Mat H;
+        H = info_show->H.clone();
+        sem.unlock();
+        if (trackers.empty())
+        {
+            usleep(1000000);
+            printf("no data available\n");
+            continue;
+        }
         frame_top = original_frame_top.clone();
         for (auto t : trackers)
         {
@@ -110,40 +409,118 @@ void *showImages(void *x_void_ptr)
                 if (pix_x < frame_top.cols && pix_y < frame_top.rows && pix_x >= 0 && pix_y >= 0)
                     cv::circle(frame_top, cv::Point(pix_x, pix_y), 7.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0);
 
-                std::vector<cv::Point2f> map_p, camera_p;
-                map_p.push_back(cv::Point2f(pix_x, pix_y));
-
-                //transform camera pixel to map pixel
-                cv::perspectiveTransform(map_p, camera_p, H.inv());
-                //std::cout << "pix_x: " << camera_p[0].x << " pix_y: " << camera_p[0].y << std::endl;
-
-                cv::circle(frame_v_loc, cv::Point(camera_p[0].x, camera_p[0].y), 3.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0);
-
-                /*if(p == t.pred_list_.size()-1)
-                {
-                    auto center = cv::Point(camera_p[0].x, camera_p[0].y);
-                    auto color = cv::Scalar(t.r_, t.g_, t.b_);
-                    draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame);
-
-                    center = cv::Point(pix_x,pix_y);
-                    draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame_top);
-                }*/
             }
         }
-
-            //outputVideo<< frame_top;
+        //outputVideo<< frame_top;
         // ------------------------------------------------
-        std::cout<<"size: "<<frame_v_loc.rows<<" - "<<frame_v_loc.cols<<std::endl;
-        std::cout<<"size: "<<frame_top.rows<<" - "<<frame_top.cols<<std::endl;
-        std::cout<<"size: "<<frame_disp_loc.rows<<" - "<<frame_disp_loc.cols<<std::endl;
-        cv::imshow("detection", frame_v_loc);
-        cv::imshow("topview", frame_top);
-        cv::imshow("disparity", frame_disp_loc);
+        mutex_t.lock();
+        updates.topview = frame_top.clone();
+        updates.update_t = true;
+        mutex_t.unlock();
 
-        cv::waitKey(1);
-        std::cout<<"visual: ";
+        std::cout<<"topviewFrame: ";
         TIMER_STOP
 
+    }    
+}
+
+void *disparityFrame(void *x_void_ptr)
+{
+    Frame_t *info_show_disparity = (Frame_t *) x_void_ptr;
+    bool first_iteration = true;
+    cv::Mat frame_loc;
+    int frame_nbr_loc = 0, pre_frame_nbr_loc = 0;
+    auto start_t = std::chrono::steady_clock::now();
+    auto step_t = std::chrono::steady_clock::now();
+    auto end_t = std::chrono::steady_clock::now();
+
+    // information for the disparity map
+    cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
+    cv::Mat canny_img;
+    cv::Mat disparity_frame;
+    while (gRun)
+    {
+        start_t = std::chrono::steady_clock::now();
+        step_t = start_t;
+        // critical section: copy the struct in local variable
+        // in this way we can unlock the sem for the main thread
+        sem_vc.lock();
+        frame_loc = info_show_disparity->frame.clone();
+        frame_nbr_loc = info_show_disparity->frame_nbr;
+        sem_vc.unlock();
+        if (frame_nbr_loc == 0)
+        {
+            usleep(1000000);
+            printf("no frame received\n");
+            continue;
+        }
+        // compute frame disparity only in there is a new frame
+        if(frame_nbr_loc - pre_frame_nbr_loc > 0)
+        {
+            pre_frame_nbr_loc = frame_nbr_loc;
+            //preprocessing frame
+            step_t = std::chrono::steady_clock::now();
+            // src_gray
+            canny_img = img_laplacian(frame_loc,0);
+            cv::Canny(canny_img, canny, 100, 100*2 );
+            // sprintf(buf_frame_crop_name,"../demo/demo/data/img_disparity/%d_%d_canny.jpg",frame_nbr_loc, 999);
+            // cv::imwrite(buf_frame_crop_name, canny);
+            end_t = std::chrono::steady_clock::now();
+            std::cout << " TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
+            step_t = end_t;
+            // std::cout<<"o: "<<frame_loc.cols<<" - "<<frame_loc.rows<<std::endl;
+            // std::cout<<"canny: "<<canny.cols<<" - "<<canny.rows<<std::endl;
+            // std::cout<<"pre: "<<pre_canny.cols<<" - "<<pre_canny.rows<<std::endl;
+            if(!first_iteration)
+            {
+                // backtorgb = cv::cvtColor(pre_canny,cv::COLOR_GRAY2RGB) 
+                cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
+                cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
+                disparity_frame = frame_disparity(pre_canny_RGB, canny_RGB, frame_nbr_loc, 999, 0);
+                // std::cout<<"size: "<<disparity_frame.rows<<" - "<<disparity_frame.cols<<std::endl;
+                // if (disparity_frame.rows == 0 || disparity_frame.cols == 0)
+                //     return -1;
+                // if (disparity_frame.empty()) 
+                // { // only fools don't check...
+                //     std::cout << "image not loaded !" << std::endl;
+                //     return -1;
+                // }
+                end_t = std::chrono::steady_clock::now();
+                std::cout << " TIME canny : frame_disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
+                step_t = end_t;
+
+                // //--------------------------------
+                // //frame box disparity on the original image
+                // step_t_segmentation = std::chrono::steady_clock::now();
+                // frame_box_disparity(pre_frame, frame, pre_rois, frame_nbr_loc);
+                // // reset pre_rois for the new roi of the current frame
+                // // pre_rois.erase(pre_rois.begin(), pre_rois.end());
+                // end_t_segmentation = std::chrono::steady_clock::now();
+                // std::cout << " TIME Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
+                // step_t_segmentation = end_t_segmentation;
+
+                // //frame box disparity on the preprocessed image
+                // cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
+                // cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
+                // frame_box_disparity(pre_canny_RGB, canny_RGB, pre_rois, frame_nbr_loc);
+                // // reset pre_rois for the new roi of the current frame
+                // pre_rois.erase(pre_rois.begin(), pre_rois.end());
+                // end_t_segmentation = std::chrono::steady_clock::now();
+                // std::cout << " TIME Canny Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
+                // step_t_segmentation = end_t_segmentation;
+                // //---------------------------------
+
+                mutex_di.lock();
+                updates.disparity = disparity_frame.clone();
+                updates.update_di = true;
+                mutex_di.unlock();
+            }
+            pre_canny = canny.clone();
+            if(first_iteration)
+                first_iteration = false;
+            end_t = std::chrono::steady_clock::now();
+            std::cout<<"disparityFrame : TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
+        }
     }
 }
 
@@ -185,17 +562,21 @@ int main(int argc, char *argv[])
     yolo.init(net);
     yolo.thresh = 0.25;
     gRun = true;
-    cv::VideoCapture cap(input);
-    if (!cap.isOpened())
-        gRun = false;
-    else
-        std::cout << "camera started\n";
 
-    pthread_t visual;
+    pthread_t visual, videocap, originalshow, detectionshow, topviewshow, disparityshow;
+    //create video capture thread
+    Frame_t info_f;
+    info_f.input = input;
+    if (pthread_create(&videocap, NULL, readVideoCapture, (void*)&info_f))
+    {
+        fprintf(stderr, "Error creating thread\n");
+        return 1;
+    };
+      
      
+
     cv::Mat frame;
     cv::Mat frame_crop;
-    cv::Mat frame_disp;
     char buf_frame_crop_name [200];
     cv::Mat dnn_input;
 
@@ -266,12 +647,13 @@ int main(int argc, char *argv[])
     auto step_t_segmentation = std::chrono::steady_clock::now();
     auto end_t_segmentation = std::chrono::steady_clock::now();
 
-    // information for the disparity map
+    //TODO: move in a thread
+    // // information for the disparity map
     std::vector <cv::Rect> pre_rois;
-    cv::Mat pre_frame;
+    // cv::Mat pre_frame;
     cv::Mat orig_frame;
-    cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
-    cv::Mat canny_img;
+    // cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
+    // cv::Mat canny_img;
 
     // box variable
     tk::dnn::box b;
@@ -282,44 +664,79 @@ int main(int argc, char *argv[])
     cv::Scalar intensity;
     cv::Rect roi;
 
-    InfoShow info_show;
+    ModFrame_t info_show;
 
     if (to_show)
     {
-        info_show.H = cv::Mat(cv::Size(3, 3), CV_64FC1);
-        if (pthread_create(&visual, NULL, showImages, (void *)&info_show))
+        // initialize updates struct
+        updates.update_o = false;
+        updates.update_de = false;
+        updates.update_t = false;
+        updates.update_di = false;
+        if (pthread_create(&visual, NULL, show_updates, (void*)NULL))
         {
             fprintf(stderr, "Error creating thread\n");
             return 1;
-        }
-    }  
+        };
+        if (pthread_create(&originalshow, NULL, originalFrame, (void*)&info_f))
+        {
+            fprintf(stderr, "Error creating thread\n");
+            return 1;
+        };
+        if (pthread_create(&disparityshow, NULL, disparityFrame, (void*)&info_f))
+        {
+            fprintf(stderr, "Error creating thread\n");
+            return 1;
+        };
+        info_show.H = cv::Mat(cv::Size(3, 3), CV_64FC1);
+        if (pthread_create(&detectionshow, NULL, detectionFrame, (void*)&info_show))
+        {
+            fprintf(stderr, "Error creating thread\n");
+            return 1;
+        };
+        if (pthread_create(&topviewshow, NULL, topviewFrame, (void*)&info_show))
+        {
+            fprintf(stderr, "Error creating thread\n");
+            return 1;
+        };
+        // if (pthread_create(&detectionshow, NULL, showImages, (void *)&info_show))
+        // {
+        //     fprintf(stderr, "Error creating thread\n");
+        //     return 1;
+        // }
+    } 
+ 
 
+    bool first_iteration = true;
     while (gRun)
     {       
         TIMER_START
         start_t = std::chrono::steady_clock::now();
         step_t = start_t;
         
-        cap >> frame;
-
-        orig_frame = frame.clone();
+        sem_vc.lock();
+        frame = info_f.frame.clone();
+        if(info_f.frame_nbr - frame_nbr > 1)
+            std::cout<<"more than one - f_n\n";
+        frame_nbr = info_f.frame_nbr;
+        sem_vc.unlock();
+        std::cout<<"f_n: "<<frame_nbr<<std::endl;
+        // if (!frame.data)
         if (frame_nbr == 0)
+        {
+            usleep(1000000);
+            printf("no frame received\n");
+            continue;
+        }
+        orig_frame = frame.clone();
+        if (first_iteration)
             cv::initUndistortRectifyMap(cameraMat, distCoeff, cv::Mat(), cameraMat, frame.size(), CV_16SC2, map1, map2);
         cv::Mat temp = frame.clone();
 
         cv::remap(temp, frame, map1, map2, 1);
-
+        
         //undistort(temp, frame, cameraMat, distCoeff);
 
-        if (!frame.data)
-        {
-            usleep(1000000);
-            cap.open(input);
-            printf("cap reinitialize\n");
-            continue;
-        }
-
-
         // this will be resized to the net format
         dnn_input = frame.clone();
         // TODO: async infer
@@ -337,58 +754,59 @@ int main(int argc, char *argv[])
         // draw dets
         std::cout<<"num detected: "<<num_detected<<std::endl;
 
-        //preprocessing frame
-        step_t_segmentation = std::chrono::steady_clock::now();
-        // src_gray
-        canny_img = img_laplacian(orig_frame,0);
-        cv::Canny(canny_img, canny, 100, 100*2 );
-        // sprintf(buf_frame_crop_name,"../demo/demo/data/img_disparity/%d_%d_canny.jpg",frame_nbr, 999);
-        // cv::imwrite(buf_frame_crop_name, canny);
-        end_t_segmentation = std::chrono::steady_clock::now();
-        std::cout << " - TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
-        step_t_segmentation = end_t_segmentation;
-        // std::cout<<"o: "<<orig_frame.cols<<" - "<<orig_frame.rows<<std::endl;
-        // std::cout<<"canny: "<<canny.cols<<" - "<<canny.rows<<std::endl;
-        // std::cout<<"pre: "<<pre_canny.cols<<" - "<<pre_canny.rows<<std::endl;
-        if(frame_nbr!=0)
-        {
-            // backtorgb = cv::cvtColor(pre_canny,cv::COLOR_GRAY2RGB) 
-            cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
-            cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
-            frame_disp = frame_disparity(pre_canny_RGB, canny_RGB, frame_nbr, 999, 0);
-            std::cout<<"size: "<<frame_disp.rows<<" - "<<frame_disp.cols<<std::endl;
-            if (frame_disp.rows == 0 || frame_disp.cols == 0)
-                return -1;
-            if (frame_disp.empty()) 
-            { // only fools don't check...
-                std::cout << "image not loaded !" << std::endl;
-                return -1;
-            }
-            end_t_segmentation = std::chrono::steady_clock::now();
-            std::cout << " TIME canny : frame_disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
-            step_t_segmentation = end_t_segmentation;
+        //TODO: move in a thread
+        // //preprocessing frame
+        // step_t_segmentation = std::chrono::steady_clock::now();
+        // // src_gray
+        // canny_img = img_laplacian(orig_frame,0);
+        // cv::Canny(canny_img, canny, 100, 100*2 );
+        // // sprintf(buf_frame_crop_name,"../demo/demo/data/img_disparity/%d_%d_canny.jpg",frame_nbr, 999);
+        // // cv::imwrite(buf_frame_crop_name, canny);
+        // end_t_segmentation = std::chrono::steady_clock::now();
+        // std::cout << " - TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
+        // step_t_segmentation = end_t_segmentation;
+        // // std::cout<<"o: "<<orig_frame.cols<<" - "<<orig_frame.rows<<std::endl;
+        // // std::cout<<"canny: "<<canny.cols<<" - "<<canny.rows<<std::endl;
+        // // std::cout<<"pre: "<<pre_canny.cols<<" - "<<pre_canny.rows<<std::endl;
+        // if(!first_iteration)
+        // {
+        //     // backtorgb = cv::cvtColor(pre_canny,cv::COLOR_GRAY2RGB) 
+        //     cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
+        //     cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
+        //     disparity_frame = frame_disparity(pre_canny_RGB, canny_RGB, frame_nbr, 999, 0);
+        //     std::cout<<"size: "<<disparity_frame.rows<<" - "<<disparity_frame.cols<<std::endl;
+        //     if (disparity_frame.rows == 0 || disparity_frame.cols == 0)
+        //         return -1;
+        //     if (disparity_frame.empty()) 
+        //     { // only fools don't check...
+        //         std::cout << "image not loaded !" << std::endl;
+        //         return -1;
+        //     }
+        //     end_t_segmentation = std::chrono::steady_clock::now();
+        //     std::cout << " TIME canny : frame_disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
+        //     step_t_segmentation = end_t_segmentation;
 
-            // //--------------------------------
-            // //frame box disparity on the original image
-            // step_t_segmentation = std::chrono::steady_clock::now();
-            // frame_box_disparity(pre_frame, frame, pre_rois, frame_nbr);
-            // // reset pre_rois for the new roi of the current frame
-            // // pre_rois.erase(pre_rois.begin(), pre_rois.end());
-            // end_t_segmentation = std::chrono::steady_clock::now();
-            // std::cout << " TIME Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
-            // step_t_segmentation = end_t_segmentation;
+        //     // //--------------------------------
+        //     // //frame box disparity on the original image
+        //     // step_t_segmentation = std::chrono::steady_clock::now();
+        //     // frame_box_disparity(pre_frame, frame, pre_rois, frame_nbr);
+        //     // // reset pre_rois for the new roi of the current frame
+        //     // // pre_rois.erase(pre_rois.begin(), pre_rois.end());
+        //     // end_t_segmentation = std::chrono::steady_clock::now();
+        //     // std::cout << " TIME Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
+        //     // step_t_segmentation = end_t_segmentation;
 
-            // //frame box disparity on the preprocessed image
-            // cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
-            // cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
-            // frame_box_disparity(pre_canny_RGB, canny_RGB, pre_rois, frame_nbr);
-            // // reset pre_rois for the new roi of the current frame
-            // pre_rois.erase(pre_rois.begin(), pre_rois.end());
-            // end_t_segmentation = std::chrono::steady_clock::now();
-            // std::cout << " TIME Canny Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
-            // step_t_segmentation = end_t_segmentation;
-            // //---------------------------------
-        }
+        //     // //frame box disparity on the preprocessed image
+        //     // cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
+        //     // cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
+        //     // frame_box_disparity(pre_canny_RGB, canny_RGB, pre_rois, frame_nbr);
+        //     // // reset pre_rois for the new roi of the current frame
+        //     // pre_rois.erase(pre_rois.begin(), pre_rois.end());
+        //     // end_t_segmentation = std::chrono::steady_clock::now();
+        //     // std::cout << " TIME Canny Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
+        //     // step_t_segmentation = end_t_segmentation;
+        //     // //---------------------------------
+        // }
 
         // compute some metrics on the whole frame
         // segmentation(pre_frame, frame, frame_nbr, 0, 0);
@@ -422,9 +840,9 @@ int main(int argc, char *argv[])
                     roi.height = (roi.y+h >= frame.rows)? frame.rows-1-roi.y : h;
                     // std::cout<<"w "<<roi.width<<" - h "<<roi.height<<std::endl;
                     // std::cout<<"wf "<<frame.cols<<" - hf "<<frame.rows<<std::endl;
-                    std::cout<<"---"<<std::endl;
-                    std::cout<<"x "<<roi.x<<" to "<<roi.width+roi.x<<" wf "<<frame.cols<<std::endl;
-                    std::cout<<"y "<<roi.y<<" to "<<roi.height+roi.y<<" hf "<<frame.rows<<std::endl;
+                    // std::cout<<"---"<<std::endl;
+                    // std::cout<<"x "<<roi.x<<" to "<<roi.width+roi.x<<" wf "<<frame.cols<<std::endl;
+                    // std::cout<<"y "<<roi.y<<" to "<<roi.height+roi.y<<" hf "<<frame.rows<<std::endl;
                     //update pre_roi for the next frame
                     pre_rois.push_back(roi);
 
@@ -433,22 +851,22 @@ int main(int argc, char *argv[])
                     /////
                     convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr);
 
-                    //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
-                    cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2);
-                    // draw label
-                    int baseline = 0;
-                    float fontScale = 0.5;
-                    int thickness = 2;
-                    cv::Size textSize = getTextSize(det_class, cv::FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
-                    cv::rectangle(frame, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), yolo.colors[b.cl], -1);
-                    cv::putText(frame, det_class, cv::Point(x0, (y0 - (baseline / 2))), cv::FONT_HERSHEY_SIMPLEX, fontScale, cv::Scalar(255, 255, 255), thickness); 
+                    // //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
+                    // cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2);
+                    // // draw label
+                    // int baseline = 0;
+                    // float fontScale = 0.5;
+                    // int thickness = 2;
+                    // cv::Size textSize = getTextSize(det_class, cv::FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
+                    // cv::rectangle(frame, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), yolo.colors[b.cl], -1);
+                    // cv::putText(frame, det_class, cv::Point(x0, (y0 - (baseline / 2))), cv::FONT_HERSHEY_SIMPLEX, fontScale, cv::Scalar(255, 255, 255), thickness); 
                 }
             }
         }
 
-        end_t = std::chrono::steady_clock::now();
-        std::cout << " TIME 2 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
-        step_t = end_t;
+        // end_t = std::chrono::steady_clock::now();
+        // std::cout << " TIME 2 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
+        // step_t = end_t;
         //convert from latitude and longitude to meters for ekf
         cur_frame.clear();
         for (size_t i = 0; i < coords.size(); i++)
@@ -456,7 +874,7 @@ int main(int argc, char *argv[])
             gc.geodetic2Enu(coords[i].lat_, coords[i].long_, 0, &east, &north, &up);
             cur_frame.push_back(Data(east, north, frame_nbr, coords[i].class_));
         }
-        if (frame_nbr == 0)
+        if (first_iteration)
         {
             for (auto f : cur_frame)
                 trackers.push_back(Tracker(f, initial_age, dt, n_states));
@@ -477,9 +895,9 @@ int main(int argc, char *argv[])
 
         if (to_show)
         {
-            //populate the InfoShow 
+            //populate the ModFrame_t 
             sem.lock();
-            info_show.frame_v = frame.clone();
+            info_show.original_frame = frame.clone();
             // std::vector<Tracker> trackers;
             info_show.trackers = trackers;
             // geodetic_converter::GeodeticConverter gc;
@@ -488,15 +906,19 @@ int main(int argc, char *argv[])
                 info_show.adfGeoTransform[i] = adfGeoTransform[i];
             // cv::Mat H;
             info_show.H = H.clone();
-            info_show.frame_disp = frame_disp.clone();
+            info_show.yolo = yolo;
+            info_show.mask = mask.clone();
             sem.unlock();
         }
 
         // update pre_frame for the disparity map
-        pre_frame = orig_frame.clone();
-        pre_canny = canny.clone();
-
+        // pre_frame = orig_frame.clone();
+        // pre_canny = canny.clone();
+        if(first_iteration)
+            first_iteration = false;
+    
         frame_nbr++;
+        std::cout<<"MAIN thread: ";
         TIMER_STOP
     }
 
diff --git a/include/classutils.h b/include/classutils.h
index ac91658..33b2be0 100644
--- a/include/classutils.h
+++ b/include/classutils.h
@@ -215,14 +215,14 @@ void addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, ge
             int pix_x, pix_y;
             coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
 
-            std::cout<<maskOrient.at<cv::Vec3b>(pix_y,pix_x)<<std::endl;
+            // std::cout<<maskOrient.at<cv::Vec3b>(pix_y,pix_x)<<std::endl;
 
             uint8_t maskOrientPixel = maskOrient.at<cv::Vec3b>(pix_y,pix_x)[0]; 
             uint8_t orientation;
             if(maskOrientPixel != 0)
             {
                 orientation = maskOrientPixel;
-                std::cout<<"orientation given by the mask "<< int(orientation)<<std::endl;
+                // std::cout<<"orientation given by the mask "<< int(orientation)<<std::endl;
             }
             else
             {