From 5a1c7fb83df36fa2e1fd5209e42090765ecf7aab Mon Sep 17 00:00:00 2001
From: Davide Sapienza <sapienza.dav@gmail.com>
Date: Thu, 26 Sep 2019 16:39:16 +0200
Subject: [PATCH] Move several cameras into a single process.

This commit pairs a camera with a thread. In this way, the
single process can to manage several camera. Every camera
thread create one video capture thread to read its input
stream. Only one camera thread can start the visualization.

This commit fixes the wrong data reading from file of the
read_projection_matrix function.

This commit fixes the wrong orientation mask accessing of
the addRoadUserfromTracker function. In the code there are
two sections to test.
---
 demo/demo/demo.cpp   | 631 +++++++++++++++++++++++++++----------------
 include/classutils.h |  93 +++++--
 2 files changed, 460 insertions(+), 264 deletions(-)

diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp
index 732c151..b6c5def 100644
--- a/demo/demo/demo.cpp
+++ b/demo/demo/demo.cpp
@@ -32,17 +32,19 @@
 #include "trackutils.h"
 #include "plot.h"
 #include "tracker.h"
+#include <assert.h> 
 
 #define MAX_DETECT_SIZE 100
 
+std::chrono::steady_clock::time_point local_clock_start;
+
+std::mutex mutexgRun;
+
 bool gRun;
 std::string obj_class[10]{"person", "car", "truck", "bus", "motor", "bike", "rider", "traffic light", "traffic sign", "train"};
 
-// 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;
+//mutex for some opencv operations
+std::mutex mutex_cv;
 
 struct ModFrame_t{
     std::vector<Tracker> trackers;
@@ -52,91 +54,150 @@ struct ModFrame_t{
     cv::Mat original_frame;
     tk::dnn::Yolo3Detection yolo;
     cv::Mat mask;
+    // sem for mainthread, detectionthread and topviewthread
+    std::mutex sem;
 };
 
 struct Frame_t{
     char *input;
     cv::Mat frame;
     int frame_nbr;
+    // sem_vc for mainthread, videocapturethread, originalthread and disparitythread
+    std::mutex sem_vc;
+};
+
+struct Camera_t{ 
+    int CAM_IDX;
+    char *input;
+    char *pmatrix;
+    char *maskfile;
+    char *cameraCalib;
+    char *maskFileOrient;
+    bool to_show;
+    tk::dnn::Yolo3Detection yolo;
+    double adfGeoTransform[6];
 };
 
 struct Show_t{
     cv::Mat original, detection, topview, disparity;
     bool update_o, update_de, update_t, update_di; 
+    // a single mutex for each operation - the show_updates function must get all mutex
+    std::mutex mutex_o, mutex_de, mutex_t, mutex_di;
 }updates;
 
 void sig_handler(int signo)
 {
     std::cout << "request gateway stop\n";
+    mutexgRun.lock();
     gRun = false;
+    mutexgRun.unlock();
 }
 
 void *readVideoCapture(void *x_void_ptr)
 {
+    std::cout<<"readVideoCapture start...\n";
+    
     Frame_t *info_f = (Frame_t *) x_void_ptr;
-    cv::VideoCapture cap(info_f->input);
-    cv::Mat frame_loc;
+    mutex_cv.lock();
+    cv::VideoCapture cap(info_f->input, cv::CAP_FFMPEG);
+    mutex_cv.unlock();
+    cv::Mat frame_loc, frame0;
     int frame_nbr_loc = 0;
     // bool to_show = false;
     if (!cap.isOpened())
+    {
+        mutexgRun.lock();
         gRun = false;
+        mutexgRun.unlock();
+    }
     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;
+    // cap.set(cv::CAP_PROP_BUFFERSIZE,3);
+    // std::cout<<"buf size: "<<cap.get(CV_CAP_PROP_BUFFERSIZE)<<std::endl;
     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;
-
+    auto current_timestamp = std::chrono::steady_clock::now();
+    
+    // compute fps and find camera's clock 
+    double shift, mean_time = 0;
+    std::cout << "Frames per second using video.get(CV_CAP_PROP_FPS) : " << cap.get(CV_CAP_PROP_FPS) << std::endl;
+    std::cout<<"readVideoCapture computes frame rate...\n";
     // //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)
+    int i=0;
+    int num_f = 120;
+    // the first 20 frames are null 
+    while (i<21)
     {        
         cap >> frame_loc;
+        i++;
+    }
+
+    i=0;
+    start_t = std::chrono::steady_clock::now();
+    while (i<num_f)
+    {        
+        step_t = std::chrono::steady_clock::now();
+        cap >> frame_loc;
+        mean_time = mean_time + std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - step_t).count();
+        std::cout << " step "<<i<<" : "<<std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - step_t).count() << " ms"<<std::endl;
+        i++;
+    }
+    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;
+    
+    /*
+    mean_time indicates the milliseconds from a frame and the next. (frame rate)
+    local_clock_sync is the camera clock.
+    shift is the difference from local camera clock and local process clock.
+    a frame is allowed if its local timestamp minus its local clock is less then a tollerance, 
+    otherwise it will be considered old.
+    */
+    auto local_clock_sync = std::chrono::steady_clock::now();
+    mean_time = mean_time/num_f;
+    shift = ((double)std::chrono::duration_cast<std::chrono::milliseconds>(local_clock_sync - local_clock_start).count()) / mean_time;
+    shift = (shift - (int)shift) * mean_time;
+    std::cout<<".-------------------------------\n";
+    std::cout<<" mean time: "<<mean_time<<std::endl;
+    std::cout<<" shift: "<<shift<<std::endl;
+    std::cout<<" TIMEDIFFERENCE:  "<<std::chrono::duration_cast<std::chrono::milliseconds>(local_clock_sync - local_clock_start).count()<<std::endl;
+    std::cout<<"\n\n\n\n";
+    std::cout<<"readVideoCapture start to capture...\n";
+    while (gRun)
+    {    
+        // mutex_cv.lock();    
+        cap >> frame_loc;
+        // mutex_cv.unlock();
+        current_timestamp = std::chrono::steady_clock::now();
+        shift = std::chrono::duration_cast<std::chrono::milliseconds>(current_timestamp - local_clock_sync).count() ;
+        std::cout << " RELATIVE TIMESTAMP FRAME : "<<shift << " ms"<<std::endl;
+        shift = shift / mean_time;
+        shift = (shift - (int)shift) * mean_time;
+        shift = (shift - mean_time/2 >= 0)? -(mean_time - shift) : shift;
+        std::cout<<"DELAY frame_"<<frame_nbr_loc<<" : "<< shift << " ms"<<std::endl;
+        // TODO: here introduce a tollerance to discard old frame
+
+        // std::cout<< "CV_CAP_PROP_POS_MSEC:   "<< cap.get( cv::CAP_PROP_POS_MSEC) <<std::endl; 
+        // std::cout<< "CV_CAP_PROP_POS_FRAMES:  "<< cap.get( cv::CAP_PROP_POS_FRAMES) <<std::endl;  // <-- the v4l2 'sequence' field
+        // std::cout<< "CV_CAP_PROP_FPS:  "<< cap.get( cv::CAP_PROP_FPS)<<std::endl;
+        // std::cout << "Format: " << cap.get(CV_CAP_PROP_FORMAT) << "\n";
         // 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;
+        std::cout<<"id: "<<cap.get(cv::CAP_PROP_FRAME_COUNT)<<std::endl;
+
         
         if (!frame_loc.data)
         {
             usleep(1000000);
+            mutex_cv.lock();
             cap.open(info_f->input);
             printf("cap reinitialize\n");
+            mutex_cv.unlock();
             continue;
         }
 
@@ -144,15 +205,17 @@ void *readVideoCapture(void *x_void_ptr)
         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->sem_vc.lock();
         info_f->frame = frame_loc.clone();
         info_f->frame_nbr = frame_nbr_loc;
-        sem_vc.unlock();
+        info_f->sem_vc.unlock();
+        // usleep(50000); 
         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 ++;
     }
+    return (void *)0;
 }
 
 /* Thread function to show the updated images 
@@ -171,40 +234,40 @@ void *show_updates(void *x_void_ptr)
         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())
+        if(updates.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();
+            updates.mutex_o.unlock();
             }
 
-        if(mutex_de.try_lock())
+        if(updates.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();
+            updates.mutex_de.unlock();
             }
 
-        if(mutex_t.try_lock())
+        if(updates.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();
+            updates.mutex_t.unlock();
             }
 
-        if(mutex_di.try_lock())
+        if(updates.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();
+            updates.mutex_di.unlock();
             }
 
         if(update_o_loc)
@@ -220,6 +283,7 @@ void *show_updates(void *x_void_ptr)
         std::cout<<"show_updates: ";
         TIMER_STOP
     }
+    return (void *)0;
 }
 
 void *originalFrame(void *x_void_ptr)
@@ -232,24 +296,25 @@ void *originalFrame(void *x_void_ptr)
         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();
+        info_show_orig->sem_vc.lock();
         frame_loc = info_show_orig->frame.clone();
         frame_nbr_loc = info_show_orig->frame_nbr;
-        sem_vc.unlock();
+        info_show_orig->sem_vc.unlock();
         if (frame_nbr_loc == 0)
         {
             usleep(1000000);
             printf("no frame received\n");
             continue;
         }
-        mutex_o.lock();
+        updates.mutex_o.lock();
         updates.original = frame_loc.clone();
         updates.update_o = true;
-        mutex_o.unlock();
+        updates.mutex_o.unlock();
         usleep(10000);  //sleep 10 msec
         std::cout<<"originalFrame: ";
         TIMER_STOP
     }
+    return (void *)0;
 }
 
 void *detectionFrame(void *x_void_ptr)
@@ -259,6 +324,7 @@ void *detectionFrame(void *x_void_ptr)
     int pix_x, pix_y;
     cv::Mat original_frame_loc;
     std::vector<Tracker> trackers;
+
     geodetic_converter::GeodeticConverter gc;
     double adfGeoTransform[6];
     cv::Mat H;
@@ -271,15 +337,19 @@ void *detectionFrame(void *x_void_ptr)
     int x0, w, x1, y0, h, y1;
     int objClass;
     std::string det_class;;
-    float prob; 
+    // float prob; 
     cv::Scalar intensity;
+    std::vector<cv::Point2f> map_p, camera_p;
+    int baseline = 0;
+    float fontScale = 0.5;
+    int thickness = 2;
 
     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();
+        info_show->sem.lock();
         original_frame_loc = info_show->original_frame.clone();
         // std::vector<Tracker> trackers;
         trackers = info_show->trackers;
@@ -291,7 +361,7 @@ void *detectionFrame(void *x_void_ptr)
         H = info_show->H.clone();
         yolo = info_show->yolo;
         mask = info_show->mask.clone();
-        sem.unlock();
+        info_show->sem.unlock();
         
         if (trackers.empty())
         {
@@ -312,7 +382,7 @@ void *detectionFrame(void *x_void_ptr)
             y1 = b.y + h;
             objClass = b.cl;
             det_class = obj_class[b.cl];
-            prob = b.prob;
+            // prob = b.prob;
 
             intensity = mask.at<uchar>(cv::Point(int(x0 + b.w / 2), y1));
             
@@ -321,9 +391,6 @@ void *detectionFrame(void *x_void_ptr)
                 //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); 
@@ -335,27 +402,35 @@ void *detectionFrame(void *x_void_ptr)
             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.clear();
+                camera_p.clear();
                 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);
+                // std::cout<<"x,y: "<<pix_x<<", "<<pix_y<<std::endl;
+                // std::cout<<"map_p: "<<map_p<<std::endl;
+                // std::cout<<"camera_p: "<<camera_p<<std::endl;
+                // std::cout<<"size original_frame_loc: "<<original_frame_loc.cols<<", "<<original_frame_loc.rows<<std::endl;
+                // assert (camera_p[0].x < original_frame_loc.cols);
+                // assert (camera_p[0].y < original_frame_loc.rows);
+                if (camera_p[0].x < original_frame_loc.cols && camera_p[0].y < original_frame_loc.rows && camera_p[0].x >= 0 && camera_p[0].y >= 0)
+                    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.mutex_de.lock();
         updates.detection = original_frame_loc.clone();
         updates.update_de = true;
-        mutex_de.unlock();
+        updates.mutex_de.unlock();
 
         std::cout<<"detectionFrame: ";
         TIMER_STOP
 
     }
+    return (void *)0;
 }
 
 void *topviewFrame(void *x_void_ptr)
@@ -366,8 +441,8 @@ void *topviewFrame(void *x_void_ptr)
     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");
+    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;
@@ -378,7 +453,7 @@ void *topviewFrame(void *x_void_ptr)
         TIMER_START
         // critical section: copy the struct in local variable
         // in this way we can unlock the sem for the main thread
-        sem.lock();
+        info_show->sem.lock();
         // std::vector<Tracker> trackers;
         trackers = info_show->trackers;
         // geodetic_converter::GeodeticConverter gc;
@@ -387,7 +462,7 @@ void *topviewFrame(void *x_void_ptr)
             adfGeoTransform[i] = info_show->adfGeoTransform[i];
         // cv::Mat H;
         H = info_show->H.clone();
-        sem.unlock();
+        info_show->sem.unlock();
         if (trackers.empty())
         {
             usleep(1000000);
@@ -399,13 +474,8 @@ void *topviewFrame(void *x_void_ptr)
         {
             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);
-                //std::cout << "lat: " << lat << " lon: " << lon << std::endl;
-                
                 coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
-
-                //std::cout << "pix_x: " << pix_x << " pix_y: " << pix_y << std::endl;
                 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);
 
@@ -413,15 +483,16 @@ void *topviewFrame(void *x_void_ptr)
         }
         //outputVideo<< frame_top;
         // ------------------------------------------------
-        mutex_t.lock();
+        updates.mutex_t.lock();
         updates.topview = frame_top.clone();
         updates.update_t = true;
-        mutex_t.unlock();
+        updates.mutex_t.unlock();
 
         std::cout<<"topviewFrame: ";
         TIMER_STOP
 
     }    
+    return (void *)0;
 }
 
 void *disparityFrame(void *x_void_ptr)
@@ -444,10 +515,10 @@ void *disparityFrame(void *x_void_ptr)
         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();
+        info_show_disparity->sem_vc.lock();
         frame_loc = info_show_disparity->frame.clone();
         frame_nbr_loc = info_show_disparity->frame_nbr;
-        sem_vc.unlock();
+        info_show_disparity->sem_vc.unlock();
         if (frame_nbr_loc == 0)
         {
             usleep(1000000);
@@ -510,10 +581,10 @@ void *disparityFrame(void *x_void_ptr)
                 // step_t_segmentation = end_t_segmentation;
                 // //---------------------------------
 
-                mutex_di.lock();
+                updates.mutex_di.lock();
                 updates.disparity = disparity_frame.clone();
                 updates.update_di = true;
-                mutex_di.unlock();
+                updates.mutex_di.unlock();
             }
             pre_canny = canny.clone();
             if(first_iteration)
@@ -522,95 +593,100 @@ void *disparityFrame(void *x_void_ptr)
             std::cout<<"disparityFrame : TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
         }
     }
+    return (void *)0;
 }
 
-int main(int argc, char *argv[])
+void *computationTask(void *x_void_ptr)
 {
-
-    std::cout << "detection\n";
-    signal(SIGINT, sig_handler);
-
-    char *net = "yolo3_coco4.rt";
-    if (argc > 1)
-        net = argv[1];
-    char *input = "../demo/demo/data/single_ped_2.mp4";
-    if (argc > 2)
-        input = argv[2];
-    char *pmatrix = "../demo/demo/data/pmundist.txt";
-    if (argc > 3)
-        pmatrix = argv[3];
-    char *tiffile = "../demo/demo/data/map_b.tif";
-    if (argc > 4)
-        tiffile = argv[4];
-    int CAM_IDX = 20936;
-    if (argc > 5)
-        CAM_IDX = atoi(argv[5]);
-    bool to_show = true;
-    if (argc > 6)
-        to_show = atoi(argv[6]);
-    char *maskfile = "../demo/demo/data/mask36.jpg";
-    if (argc > 7)
-        maskfile = argv[7];
-    char *cameraCalib = "../demo/demo/data/calib36.params";
-    if (argc > 8)
-        cameraCalib = argv[8];
-    char *maskFileOrient = "../demo/demo/data/mask_orient/6315_mask_orient.jpg";
-    if (argc > 9)
-        maskFileOrient = argv[9];
-
-    tk::dnn::Yolo3Detection yolo;
-    yolo.init(net);
-    yolo.thresh = 0.25;
-    gRun = true;
-
-    pthread_t visual, videocap, originalshow, detectionshow, topviewshow, disparityshow;
+    Camera_t *camera = (Camera_t *) x_void_ptr;
+    pthread_t visual, originalshow, detectionshow, topviewshow, disparityshow;
+    pthread_t videocap;
     //create video capture thread
     Frame_t info_f;
-    info_f.input = input;
+    info_f.input = camera->input;
     if (pthread_create(&videocap, NULL, readVideoCapture, (void*)&info_f))
     {
         fprintf(stderr, "Error creating thread\n");
-        return 1;
+        return (void *)1;
     };
+
+    bool to_show = camera->to_show;
+    double adfGeoTransform[6];
+    for(int i=0; i<6; i++)
+        adfGeoTransform[i] = camera->adfGeoTransform[i];
+    
+    ModFrame_t info_show;
+    if (to_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 (void *)1;
+        };
+        if (pthread_create(&originalshow, NULL, originalFrame, (void*)&info_f))
+        {
+            fprintf(stderr, "Error creating thread\n");
+            return (void *)1;
+        };
+        if (pthread_create(&disparityshow, NULL, disparityFrame, (void*)&info_f))
+        {
+            fprintf(stderr, "Error creating thread\n");
+            return (void *)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 (void *)1;
+        };
+        if (pthread_create(&topviewshow, NULL, topviewFrame, (void*)&info_show))
+        {
+            fprintf(stderr, "Error creating thread\n");
+            return (void *)1;
+        };
+    } 
       
-     
-
-    cv::Mat frame;
-    cv::Mat frame_crop;
-    char buf_frame_crop_name [200];
-    cv::Mat dnn_input;
-
+    char* pmatrix = camera->pmatrix;
     /*projection matrix from camera to map*/
     cv::Mat H(cv::Size(3, 3), CV_64FC1);
     read_projection_matrix(H, pmatrix);
-
+    assert (cv::countNonZero(H) > 0);
+    // std::cout<<H<<std::endl;
+    // return (void*)0;
     /*Camera calibration*/
     cv::Mat cameraMat, distCoeff;
-    readCameraCalibrationYaml(cameraCalib, cameraMat, distCoeff);
+    readCameraCalibrationYaml(camera->cameraCalib, cameraMat, distCoeff);
     std::cout << cameraMat << std::endl;
     std::cout << distCoeff << std::endl;
 
     /*GPS information*/
-    double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
-    readTiff(tiffile, adfGeoTransform);
     std::vector<ObjCoords> coords;
-
+    
     /*socket*/
     Communicator Comm(SOCK_DGRAM);
-    Comm.open_client_socket("127.0.0.1", 8888);
+    Comm.open_client_socket((char *)"127.0.0.1", 8888);
+    
     Message *m = new Message;
-    m->cam_idx = CAM_IDX;
+    m->cam_idx = camera->CAM_IDX;
     m->lights.clear();
-
+    std::cout<<"COMM\n";
     /*Conversion for tracker, from gps to meters and viceversa*/
+    // mutex_cv.lock();
     geodetic_converter::GeodeticConverter gc;
     gc.initialiseReference(44.655540, 10.934315, 0);
+    // mutex_cv.unlock();
     double east, north, up;
     // double lat, lon, alt;
-
+    std::cout<<"COMMINT\n";
     /*Mask info*/
-    cv::Mat mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE);
-    cv::Mat maskOrient = cv::imread(maskFileOrient);
+    cv::Mat mask = cv::imread(camera->maskfile, cv::IMREAD_GRAYSCALE);
+    cv::Mat maskOrient = cv::imread(camera->maskFileOrient);
+    // cv::Mat maskOrient = cv::imread(camera->maskFileOrient, 0);
 
     /*for(int i=0; i< mask.cols; i++)
     {
@@ -620,11 +696,10 @@ int main(int argc, char *argv[])
         }
     }
     
-
+   
     return 0;*/
-
+    std::cout<<"COMM2\n";
     /*tracker infos*/
-    srand(time(NULL));
     std::vector<Tracker> trackers;
     std::vector<Data> cur_frame;
     int initial_age = -5;
@@ -644,12 +719,12 @@ int main(int argc, char *argv[])
     auto start_t = std::chrono::steady_clock::now();
     auto step_t = std::chrono::steady_clock::now();
     auto end_t = std::chrono::steady_clock::now();
-    auto step_t_segmentation = std::chrono::steady_clock::now();
-    auto end_t_segmentation = std::chrono::steady_clock::now();
+    // auto step_t_segmentation = std::chrono::steady_clock::now();
+    // auto end_t_segmentation = std::chrono::steady_clock::now();
 
     //TODO: move in a thread
     // // information for the disparity map
-    std::vector <cv::Rect> pre_rois;
+    // std::vector <cv::Rect> pre_rois;
     // cv::Mat pre_frame;
     cv::Mat orig_frame;
     // cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
@@ -657,56 +732,15 @@ int main(int argc, char *argv[])
 
     // box variable
     tk::dnn::box b;
-    int x0, w, x1, y0, h, y1;
+    int x0, h, y1; //w, x1, y0; 
     int objClass;
     std::string det_class;;
-    float prob; 
+    // float prob; 
     cv::Scalar intensity;
-    cv::Rect roi;
-
-    ModFrame_t info_show;
-
-    if (to_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;
-        // }
-    } 
- 
 
+    cv::Mat frame;
+    cv::Mat frame_crop;
+    cv::Mat dnn_input;
     bool first_iteration = true;
     while (gRun)
     {       
@@ -714,12 +748,12 @@ int main(int argc, char *argv[])
         start_t = std::chrono::steady_clock::now();
         step_t = start_t;
         
-        sem_vc.lock();
+        info_f.sem_vc.lock();
         frame = info_f.frame.clone();
         if(info_f.frame_nbr - frame_nbr > 1)
-            std::cout<<"more than one - f_n\n";
+            std::cout<<"more than one - f_n (diff "<<info_f.frame_nbr - frame_nbr<<")\n";
         frame_nbr = info_f.frame_nbr;
-        sem_vc.unlock();
+        info_f.sem_vc.unlock();
         std::cout<<"f_n: "<<frame_nbr<<std::endl;
         // if (!frame.data)
         if (frame_nbr == 0)
@@ -729,30 +763,28 @@ int main(int argc, char *argv[])
             continue;
         }
         orig_frame = frame.clone();
+        // mutex_cv.lock();
         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);
+        // mutex_cv.unlock();
 
         // this will be resized to the net format
         dnn_input = frame.clone();
         // TODO: async infer
-        yolo.update(dnn_input);
-
-        int num_detected = yolo.detected.size();
+        camera->yolo.update(dnn_input);
+        int num_detected = camera->yolo.detected.size();
         if (num_detected > MAX_DETECT_SIZE)
             num_detected = MAX_DETECT_SIZE;
 
         coords.clear();
-
         end_t = std::chrono::steady_clock::now();
         std::cout << " TIME 1 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
         step_t = end_t;
         // draw dets
-        std::cout<<"num detected: "<<num_detected<<std::endl;
+        std::cout<<"camera: "<<camera->CAM_IDX <<" - num detected: "<<num_detected<<std::endl;
 
         //TODO: move in a thread
         // //preprocessing frame
@@ -813,16 +845,16 @@ int main(int argc, char *argv[])
 
         for (int i = 0; i < num_detected; i++)
         {
-            b = yolo.detected[i];
+            b = camera->yolo.detected[i];
             x0 = b.x;
-            w = b.w;
-            x1 = b.x + w;
-            y0 = b.y;
+            // 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;
+            // prob = b.prob;
 
             intensity = mask.at<uchar>(cv::Point(int(x0 + b.w / 2), y1));
             
@@ -833,47 +865,50 @@ int main(int argc, char *argv[])
                 {
 
                     // find the rectangular on the frame (sub-figure)                        
-                    roi.x = (x0 > 0)? x0 : 0;
-                    roi.y = (y0 > 0)? y0 : 0;
-                    // std::cout<<"x "<<roi.x<<" - y "<<roi.y<<std::endl;
-                    roi.width = (roi.x+w >= frame.cols)? frame.cols-1-roi.x : w;
-                    roi.height = (roi.y+h >= frame.rows)? frame.rows-1-roi.y : h;
+                    // roi.x = (x0 > 0)? x0 : 0;
+                    // roi.y = (y0 > 0)? y0 : 0;
+                    // // std::cout<<"x "<<roi.x<<" - y "<<roi.y<<std::endl;
+                    // roi.width = (roi.x+w >= frame.cols)? frame.cols-1-roi.x : w;
+                    // 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;
                     //update pre_roi for the next frame
-                    pre_rois.push_back(roi);
+                    // pre_rois.push_back(roi);
 
                     // segmentation(frame(roi), frame(roi), frame_nbr, i, 1);
 
                     /////
                     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);
+                    // cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), camera->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::rectangle(frame, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), camera->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();
+        assert (!coords.empty());
         for (size_t i = 0; i < coords.size(); i++)
         {
             gc.geodetic2Enu(coords[i].lat_, coords[i].long_, 0, &east, &north, &up);
             cur_frame.push_back(Data(east, north, frame_nbr, coords[i].class_));
         }
+        assert (!cur_frame.empty());
         if (first_iteration)
         {
             for (auto f : cur_frame)
@@ -883,20 +918,25 @@ int main(int argc, char *argv[])
         {
             Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
         }
-
         std::cout << "There are " << trackers.size() << " trackers" << std::endl;
-
         //prepare message with tracker info
-        addRoadUserfromTracker(trackers, m, gc, maskOrient, adfGeoTransform);
+        assert (trackers.size()!=0);
+        // mutex_cv.lock();
+        addRoadUserfromTracker(trackers, m, gc, maskOrient, adfGeoTransform, H);
+        // mutex_cv.unlock();
+        
         //prepare the message with detection info
         //prepare_message(m, coords, CAM_IDX);
         //send message
-        Comm.send_message(m);
-
+        // assert (!m->objects.empty());
+        if (!m->objects.empty())
+            Comm.send_message(m);
+        // assert (trackers.size()!=0);
+        
         if (to_show)
         {
             //populate the ModFrame_t 
-            sem.lock();
+            info_show.sem.lock();
             info_show.original_frame = frame.clone();
             // std::vector<Tracker> trackers;
             info_show.trackers = trackers;
@@ -906,9 +946,10 @@ int main(int argc, char *argv[])
                 info_show.adfGeoTransform[i] = adfGeoTransform[i];
             // cv::Mat H;
             info_show.H = H.clone();
-            info_show.yolo = yolo;
+            info_show.yolo = camera->yolo;
+            // std::copy(camera->yolo.begin(), camera->yolo.end(), info_show.yolo.begin());
             info_show.mask = mask.clone();
-            sem.unlock();
+            info_show.sem.unlock();
         }
 
         // update pre_frame for the disparity map
@@ -918,12 +959,134 @@ int main(int argc, char *argv[])
             first_iteration = false;
     
         frame_nbr++;
-        std::cout<<"MAIN thread: ";
+        std::cout<<camera->CAM_IDX<<" camera thread: ";
         TIMER_STOP
     }
+    return (void *)0;
+}
 
+int main(int argc, char *argv[])
+{
+
+    std::cout << "detection\n";
+    signal(SIGINT, sig_handler);
+    srand(time(NULL));
+    bool no_params = false; //flag to indicate if there are camera parameters or we use mp4 test video 
+
+    char *net = (char *)"yolo3_coco4.rt";
+    if (argc > 1)
+        net = argv[1];
+    char *tiffile = (char *)"../demo/demo/data/map_b.tif";
+    if (argc > 2)
+        tiffile = argv[2];
+    char *n;
+    if (argc > 3)
+    {
+        n = argv[3];
+        if(strcmp(n, "-n"))
+            return -1;    
+    };
+    int n_cameras = 0;
+    if (argc > 4)
+    {
+        n_cameras = atoi(argv[4]);
+        if(argc < 5+ 7 * n_cameras)
+        {
+            std::cout<<"too few parameters\n";
+            return -1;
+        }
+        if(!n_cameras)
+        {
+            n_cameras = 1;
+            no_params = true;
+        }        
+    }
+    Camera_t cameras[n_cameras]; 
+    bool *to_show = (bool *) malloc(n_cameras * sizeof(bool)); 
+    if(no_params) 
+    {
+        cameras[0].CAM_IDX = 20936;
+        cameras[0].input = (char *)"../demo/demo/data/single_ped_2.mp4";
+        cameras[0].pmatrix = (char *)"../demo/demo/data/pmundist.txt";
+        cameras[0].maskfile = (char *)"../demo/demo/data/mask36.jpg";
+        cameras[0].cameraCalib = (char *)"../demo/demo/data/calib36.params";
+        cameras[0].maskFileOrient = (char *)"../demo/demo/data/mask_orient/6315_mask_orient.jpg";
+        cameras[0].to_show = true;
+        to_show[0] = true;
+    }
+    else
+    {
+        for(int i = 0; i<n_cameras; i++)
+            cameras[i].CAM_IDX = atoi(argv[5+i]);
+        for(int i = 0; i<n_cameras; i++)
+            cameras[i].input = argv[5+n_cameras+i];
+        for(int i = 0; i<n_cameras; i++)
+            cameras[i].pmatrix = argv[5+2*n_cameras+i];
+        for(int i = 0; i<n_cameras; i++)
+            cameras[i].maskfile = argv[5+3*n_cameras+i];
+        for(int i = 0; i<n_cameras; i++)
+            cameras[i].cameraCalib = argv[5+4*n_cameras+i];
+        for(int i = 0; i<n_cameras; i++)
+            cameras[i].maskFileOrient = argv[5+5*n_cameras+i];
+        for(int i = 0; i<n_cameras; i++)
+        {
+            cameras[i].to_show = atoi(argv[5+6*n_cameras+i]); //only one camera can be shown
+            to_show[i] = atoi(argv[5+6*n_cameras+i]);
+        }
+    }
+    //TODO now only one camera can be visualized
+    int check_visualization=0;
+    for(int i = 0; i<n_cameras; i++)
+        check_visualization += to_show[i];
+    if(check_visualization > 1)
+        return -1;
+
+    
+    tk::dnn::Yolo3Detection yolo[n_cameras];
+    for(int i=0; i<n_cameras; i++)
+    {
+        yolo[i].init(net);
+        yolo[i].thresh = 0.25;    
+    }
+    // tk::dnn::Yolo3Detection yolo;
+    // yolo.init(net);
+    // yolo.thresh = 0.25;    
+    
+    gRun = true;
+
+    // start the local clock. It is used to check the incoming frames (by different cameras) 
+    local_clock_start = std::chrono::steady_clock::now();
+
+    /*GPS information*/
+    double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
+    readTiff(tiffile, adfGeoTransform);
+   
+    for(int i=0; i<n_cameras; i++)
+    {
+        for(int j = 0; j < 6; j++ )
+                cameras[i].adfGeoTransform[j] = adfGeoTransform[j];
+        cameras[i].yolo = yolo[i];
+        // cameras[i].yolo = yolo;
+        
+    }
+    std::cout<<"INIZIA:\n";
+    pthread_t camera_task[n_cameras];
+    for(int i=0; i<n_cameras; i++)
+    {
+        std::cout<<"creating thread\n";
+        if(pthread_create(&camera_task[i], NULL, computationTask, (void*)&cameras[i]))
+        {
+            fprintf(stderr, "error creating thread\n");
+            return 1;
+        }
+        std::cout<<"WHAT2\n";
+    }
+    for(int i=0; i<n_cameras; i++)
+    {
+        pthread_join(camera_task[i], NULL);
+    }
+    std::cout <<" free adfGeoT \n";
     free(adfGeoTransform);
-
     std::cout << "detection end\n";
     return 0;
 }
diff --git a/include/classutils.h b/include/classutils.h
index 33b2be0..8bf1e46 100644
--- a/include/classutils.h
+++ b/include/classutils.h
@@ -85,7 +85,7 @@ void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform
     y = int(round((lat - adfGeoTransform[3]) / adfGeoTransform[5]));
 }
 
-void fillMatrix(cv::Mat &H, float *matrix, bool show = false)
+void fillMatrix(cv::Mat &H, double *matrix, bool show = false)
 {
     double *vals = (double *)H.data;
     for (int i = 0; i < 9; i++)
@@ -103,7 +103,6 @@ void convert_coords(std::vector<ObjCoords> &coords, int x, int y, int detected_c
     double latitude, longitude;
     std::vector<cv::Point2f> x_y, ll;
     x_y.push_back(cv::Point2f(x, y));
-
     //transform camera pixel to map pixel
     cv::perspectiveTransform(x_y, ll, H);
     //tranform to map pixel to map gps
@@ -136,25 +135,24 @@ void read_projection_matrix(cv::Mat &H, char *path)
     size_t len = 0;
     ssize_t read;
 
-    float *proj_matrix = (float *)malloc(9 * sizeof(float));
-
+    // float *proj_matrix = (float *)malloc(9 * sizeof(float));
+    double proj_matrix[9] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    int i = 0;
     fp = fopen(path, "r");
     if (fp == NULL)
         exit(EXIT_FAILURE);
-
-    int i = 0;
+        
     while ((read = getline(&line, &len, fp)) != -1)
     {
-        if (3 == sscanf(line, "%f %f %f", &proj_matrix[i * 3 + 0], &proj_matrix[i * 3 + 1], &proj_matrix[i * 3 + 2]))
-        {
+        std::cout<<line<<std::endl;
+        std::stringstream ss(line);
+        while (ss >> proj_matrix[i])
             i++;
-        }
     }
-    free(line);
     fclose(fp);
     fillMatrix(H, proj_matrix);
-
-    free(proj_matrix);
+    free(line);
+    // free(proj_matrix);
 }
 
 void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, cv::Mat &frame)
@@ -176,10 +174,9 @@ unsigned long long time_in_ms()
     return t_stamp_ms;
 }
 
-void addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, geodetic_converter::GeodeticConverter &gc, const cv::Mat& maskOrient, double *adfGeoTransform)
+void addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, geodetic_converter::GeodeticConverter &gc, const cv::Mat& maskOrient, double *adfGeoTransform, cv::Mat H)
 {
     m->t_stamp_ms = time_in_ms();
-    m->num_objects = trackers.size();
     m->objects.clear();
     double lat, lon, alt;
 
@@ -215,29 +212,65 @@ 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;
+            // TODO: test correctness - added perspective transform call to converter pix_x and pix_y
+            // sometimes some values are wrong. float ok? 
+            // std::vector<cv::Point2f> map_p, camera_p;
+            // std::cout<<"--- pix_x, pix_y: "<<pix_x<<", "<<pix_y<<std::endl;
+            // map_p.push_back(cv::Point2f(pix_x, pix_y));
+            // std::cout<<"map_p: "<<map_p<<std::endl;
+            // //transform camera pixel to map pixel
+            // cv::perspectiveTransform(map_p, camera_p, H.inv());
+            // std::cout<<"size H: "<<H.cols<<", "<<H.rows<<std::endl;
+            // std::cout<<"camera_p: "<<camera_p<<std::endl;    
+            // // TODO: in some cases these lines causes seg fault!
+            // std::cout<<"y, x :"<<camera_p[0].y<<",  "<<camera_p[0].x<<std::endl;
+            // std::cout<<"size maskorient: "<<maskOrient.cols<<", "<<maskOrient.rows<<std::endl;
+            // // std::cout<<"vec3b: "<<(cv::Vec3b)(pix_y,pix_x);
+            // assert (camera_p[0].x < maskOrient.cols);
+            // assert (camera_p[0].y < maskOrient.rows);
+            // uint8_t maskOrientPixel = maskOrient.at<cv::Vec3b>(camera_p[0].y,camera_p[0].x)[0]; 
+            // std::cout<<"boo: "<<maskOrient.at<cv::Vec3b>(camera_p[0].y,camera_p[0].x)<<std::endl;
+            // uint8_t orientation;
+            // if(maskOrientPixel != 0)
+            // {
+            //     orientation = maskOrientPixel;
+            //     // std::cout<<"orientation given by the mask "<< int(orientation)<<std::endl;
+            // }
+            // else
+            // {
+            //     orientation = uint8_t((int((t.pred_list_.back().yaw_ * 57.29 + 360)) % 360) * 17 / 24);
+            //     //std::cout<<"orientation given by the tracker "<< int(orientation)<<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;
-            }
-            else
-            {
-                orientation = uint8_t((int((t.pred_list_.back().yaw_ * 57.29 + 360)) % 360) * 17 / 24);
-                //std::cout<<"orientation given by the tracker "<< int(orientation)<<std::endl;
-            }
-            
+            // TODO: to validate -> it works for grayscale image (see demo.cpp, row: "cv::Mat maskOrient = cv::imread(camera->maskFileOrient, 0);")
+            // TODO: include perspective transform
+            // std::cout<<"y, x :"<<pix_y<<", "<<pix_x<<std::endl;
+            // std::cout<<"size maskorient: "<<maskOrient.cols<<", "<<maskOrient.rows<<std::endl;
+            // std::cout<<"point: "<<(cv::Point)(pix_y,pix_x);
+            // uint8_t maskOrientPixel = maskOrient.at<uchar>(pix_y,pix_x);
+            // uint8_t orientation;
+            // if(maskOrientPixel != 0)
+            // {
+            //     orientation = maskOrientPixel;
+            //     // std::cout<<"orientation given by the mask "<< int(orientation)<<std::endl;
+            // }
+            // else
+            // {
+            //     orientation = uint8_t((int((t.pred_list_.back().yaw_ * 57.29 + 360)) % 360) * 17 / 24);
+            //     //std::cout<<"orientation given by the tracker "<< int(orientation)<<std::endl;
+            // }
+
+            uint8_t orientation = uint8_t((int((t.pred_list_.back().yaw_ * 57.29 + 360)) % 360) * 17 / 24);
+            // std::cout<<"orient: "<<unsigned(orientation)<<std::endl;
             //std::cout << "lat: " << lat << " lon: " << lon << std::endl;
             uint8_t velocity = uint8_t(std::abs(t.pred_list_.back().vel_ * 3.6 / 2));
-            
+            // std::cout<<"vel: "<<unsigned(velocity)<<std::endl;
             RoadUser r{static_cast<float>(lat), static_cast<float>(lon), velocity, orientation, cat};
             //std::cout << std::setprecision(10) << r.latitude << " , " << r.longitude << " " << int(r.speed) << " " << int(r.orientation) << " " << r.category << std::endl;
             m->objects.push_back(r);
         }
     }
+    m->num_objects = m->objects.size();
 }
 
 void prepare_message(Message *m, const std::vector<ObjCoords> &coords, int idx)
@@ -247,7 +280,7 @@ void prepare_message(Message *m, const std::vector<ObjCoords> &coords, int idx)
     m->num_objects = coords.size();
 
     m->objects.clear();
-    for (int i = 0; i < coords.size(); i++)
+    for (unsigned int i = 0; i < coords.size(); i++)
     {
         Categories cat;