From 5f444825ada7d0a7a58d992d6cd831a1e6dbd7be Mon Sep 17 00:00:00 2001
From: mive93 <micaelaverucchi@gmail.com>
Date: Thu, 16 May 2019 19:48:59 +0200
Subject: [PATCH] optimized undistortion

---
 demo/demo/demo.cpp   | 37 +++++++++++++++++++++++++++----------
 include/classutils.h | 25 ++++++++++++++++++++++---
 2 files changed, 49 insertions(+), 13 deletions(-)

diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp
index c59f426..46aeef1 100644
--- a/demo/demo/demo.cpp
+++ b/demo/demo/demo.cpp
@@ -79,6 +79,9 @@ int main(int argc, char *argv[])
     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);
@@ -137,6 +140,18 @@ int main(int argc, char *argv[])
 
     /*Mask info*/
     cv::Mat mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE);
+    cv::Mat maskOrient = cv::imread(maskFileOrient);
+
+    /*for(int i=0; i< mask.cols; i++)
+    {
+        for(int j=0; j< mask.rows; j++)
+        {
+            std::cout<<maskOrient.at<cv::Vec3b>(i,j)    <<std::endl;
+        }
+    }
+    
+
+    return 0;*/
 
     /*tracker infos*/
     srand(time(NULL));
@@ -155,14 +170,21 @@ int main(int argc, char *argv[])
                           (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT));
     outputVideo.open("test.avi", static_cast<int>(cap.get(cv::CAP_PROP_FOURCC)), cap.get(cv::CAP_PROP_FPS), S, true);*/
 
+    cv::Mat map1, map2;
+
     while (gRun)
     {
-
+        TIMER_START
         cap >> frame;
 
+        if (frame_nbr == 0)
+            cv::initUndistortRectifyMap(cameraMat, distCoeff, cv::Mat(), cameraMat, frame.size(), CV_16SC2, map1, map2);
         cv::Mat temp = frame.clone();
-        undistort(temp, frame, cameraMat, distCoeff);
-        cv::imwrite(std::to_string(CAM_IDX) + ".jpg", frame);
+
+        cv::remap(temp, frame, map1, map2, 1);
+
+        //undistort(temp, frame, cameraMat, distCoeff);
+
 
         if (!frame.data)
         {
@@ -218,8 +240,6 @@ int main(int argc, char *argv[])
             }
         }
 
-        TIMER_START
-
         //convert from latitude and longitude to meters for ekf
         cur_frame.clear();
         for (size_t i = 0; i < coords.size(); i++)
@@ -237,18 +257,15 @@ int main(int argc, char *argv[])
             Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
         }
 
-        TIMER_STOP
         std::cout << "There are " << trackers.size() << " trackers" << std::endl;
 
-
         //prepare message with tracker info
-        addRoadUserfromTracker(trackers, m, gc);
+        addRoadUserfromTracker(trackers, m, gc, maskOrient, adfGeoTransform);
         //prepare the message with detection info
         //prepare_message(m, coords, CAM_IDX);
         //send message
         Comm.send_message(m);
 
-
         if (to_show)
         {
             frame_top = original_frame_top.clone();
@@ -308,7 +325,7 @@ int main(int argc, char *argv[])
         }
 
         frame_nbr++;
-        
+        TIMER_STOP
     }
 
     free(adfGeoTransform);
diff --git a/include/classutils.h b/include/classutils.h
index 31f04c0..ac91658 100644
--- a/include/classutils.h
+++ b/include/classutils.h
@@ -176,7 +176,7 @@ unsigned long long time_in_ms()
     return t_stamp_ms;
 }
 
-void addRoadUserfromTracker(const std::vector<Tracker>& trackers, Message *m, geodetic_converter::GeodeticConverter& gc)
+void addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, geodetic_converter::GeodeticConverter &gc, const cv::Mat& maskOrient, double *adfGeoTransform)
 {
     m->t_stamp_ms = time_in_ms();
     m->num_objects = trackers.size();
@@ -211,11 +211,30 @@ void addRoadUserfromTracker(const std::vector<Tracker>& trackers, Message *m, ge
             }
             //std::cout << t.pred_list_.size() << std::endl;
             gc.enu2Geodetic(t.pred_list_.back().x_, t.pred_list_.back().y_, 0, &lat, &lon, &alt);
+
+            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;
+
+            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;
+            }
+            
             //std::cout << "lat: " << lat << " lon: " << lon << std::endl;
             uint8_t velocity = uint8_t(std::abs(t.pred_list_.back().vel_ * 3.6 / 2));
-            uint8_t orientation = uint8_t((int((t.pred_list_.back().yaw_ * 57.29 + 360)) % 360) * 17 / 24);
+            
             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;
+            //std::cout << std::setprecision(10) << r.latitude << " , " << r.longitude << " " << int(r.speed) << " " << int(r.orientation) << " " << r.category << std::endl;
             m->objects.push_back(r);
         }
     }