From cfb457fdec787ca1addec12e9be6c8921ca8451a Mon Sep 17 00:00:00 2001
From: Davide Sapienza <177992@studenti.unimore.it>
Date: Mon, 11 Nov 2019 18:07:03 +0100
Subject: [PATCH] Fix bug in velocity conversion.

This commit fixes a wrong operation in the velocity conversion.
A reduced speed (because we are in a urban track) is now stored
into a uint8. Thus granularity is now half km/h.

Signed-off-by: Davide Sapienza <sapienza.dav@gmail.com>
---
 CMakeLists.txt            |  2 +-
 src/class_src/message.cpp | 31 +++++++++++++++++++++++++++++--
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index bb87f2e..c1f86fa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -41,7 +41,7 @@ set(tkdnn_LIBS kernels ${CUDA_LIBRARIES} ${CUDA_CUBLAS_LIBRARIES} -lcudnn -lnvin
 file(GLOB class_SRC "src/class_src/*.cpp")
 set(class_LIBS ${OpenCV_LIBS} -lgdal yaml-cpp python2.7)
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11 -O3")
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} ${NVINFER_INCLUDES} "~/repos/cereal/include" ${CMAKE_CURRENT_SOURCE_DIR}/tracker_CLASS/c++/src)
 include_directories( BEFORE ${MY_SOURCE_DIR}/src /usr/include/python2.7 )
 
diff --git a/src/class_src/message.cpp b/src/class_src/message.cpp
index b9e2f08..96d3200 100644
--- a/src/class_src/message.cpp
+++ b/src/class_src/message.cpp
@@ -9,6 +9,32 @@ unsigned long long time_in_ms()
     return t_stamp_ms;
 }
 
+/* Convert orientation from radian to quantized degree (from 360 to 255)
+**/
+uint8_t orientation_to_uint8(float yaw)
+{
+    // 57.29 is (180/pi) -> conversion in degrees
+    // 17 / 24 is (255/360) -> quantization
+    // let a yaw in radians, it is converted into degrees (*57.29), 
+    // then into positive degrees, then it is quantized into 255. 
+    uint8_t orientation = uint8_t((int((yaw * 57.29 + 360)) % 360) * 17 / 24);
+    return orientation;
+}
+
+/* Convert speed from m/s to quantized km/h every 1/2 km/h
+**/
+uint8_t speed_to_uint8(float vel)
+{
+    // 3.6 -> conversion in km/h
+    // *2 -> quantization km/h evrey 1/2
+    // let a velocity in m/s, it is converted into km/h (*3.6), then (*2) 
+    // we achive a double speed. In a urban track we can consider a maximum 
+    // speed of 127 km/h. So we can fit 127 on a byte with a multiplication 
+    // by 2. Each increment corresponds to a speed greater than 1/2 km/h.
+    uint8_t velocity = uint8_t(std::abs(vel * 3.6 * 2));
+    return velocity;
+}        
+
 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();
@@ -95,10 +121,11 @@ void addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, ge
             //     //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);
+            uint8_t orientation = orientation_to_uint8(t.pred_list_.back().yaw_);
             // 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));
+            uint8_t velocity = speed_to_uint8(t.pred_list_.back().vel_);
+            
             // 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;