Udpate tracker

Signed-off-by: Micaela Verucchi <micaelaverucchi@gmail.com>

.gitignore

@@ -1,4 +1,10 @@
 *~
+demo/demo/data/img_crop/
+demo/demo/data/img_disparity/
+demo/demo/data/map/
+demo/demo/data/masks_orient/
+demo/demo/data/pmat_new/
+demo/demo/data/masks_v2/
 build/
 .vscode/
 *.bin
@@ -8,4 +14,6 @@ build/
 *.h5
 *.tar.gz
 *.weights
+*.zip
 .idea/
+*.hdf5

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "tracker_CLASS"]
+	path = tracker_CLASS
+	url = https://github.com/mive93/tracker_CLASS.git
+[submodule "masa_protocol"]
+	path = masa_protocol
+	url = https://git.hipert.unimore.it/rcavicchioli/masa_protocol.git

CMakeLists.txt

@@ -32,6 +32,7 @@ cuda_add_library(kernels SHARED ${tkdnn_CUSRC})
 find_package(OpenCV REQUIRED)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DOPENCV")
 
+include_directories(/usr/include/gdal)
 
 #-------------------------------------------------------------------------------
 # Build Libraries
@@ -39,11 +40,20 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DOPENCV")
 file(GLOB tkdnn_SRC "src/*.cpp")
 set(tkdnn_LIBS kernels ${CUDA_LIBRARIES} ${CUDA_CUBLAS_LIBRARIES} ${CUDNN_LIBRARIES} ${OpenCV_LIBS})
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
+file(GLOB class_SRC "src/class_src/*.cpp")
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} ${NVINFER_INCLUDES})
+
+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 /usr/include/python2.7)
+
+set(class_LIBS ${OpenCV_LIBS} -lgdal yaml-cpp python2.7)
+
 add_library(tkDNN SHARED ${tkdnn_SRC})
 target_link_libraries(tkDNN ${tkdnn_LIBS})
 
+add_library(CLASS SHARED ${class_SRC})
+target_link_libraries(CLASS ${class_LIBS})
+
+
 #static 
 #add_library(tkDNN_static STATIC ${tkdnn_SRC})
 #target_link_libraries(tkDNN_static ${tkdnn_LIBS})
@@ -83,16 +93,35 @@ target_link_libraries(test_yolo3_coco4 tkDNN)
 add_executable(test_yolo3_berkeley tests/yolo3_berkeley/yolo3_berkeley.cpp)
 target_link_libraries(test_yolo3_berkeley tkDNN)
 
+add_executable(test_yolo3_tetrapack tests/yolo3_tetrapack/yolo3_tetrapack.cpp)
+target_link_libraries(test_yolo3_tetrapack tkDNN)
+
+add_executable(test_yolo3_tetrapack_resize tests/yolo3_tetrapack_resize/yolo3_tetrapack_resize.cpp)
+target_link_libraries(test_yolo3_tetrapack_resize tkDNN)
+
+add_executable(test_yolo3_BCDS6 tests/yolo3_BCDS6/yolo3_BCDS6.cpp)
+target_link_libraries(test_yolo3_BCDS6 tkDNN)
 add_executable(test_yolo3_flir tests/yolo3_flir/yolo3_flir.cpp)
 target_link_libraries(test_yolo3_flir tkDNN)
+
+add_executable(test_imuodom tests/imuodom/imuodom.cpp)
+target_link_libraries(test_imuodom tkDNN)
 ################################################################################
 
 
 add_executable(test_rtinference tests/test_rtinference/rtinference.cpp)
 target_link_libraries(test_rtinference tkDNN)
 
-add_executable(yolo3_demo demo/demo/demo.cpp)
+add_executable(yolo3_demo demo/demo/demo.cpp 
-target_link_libraries(yolo3_demo tkDNN)
+                        tracker_CLASS/c++/src/ekf.cpp
+                        tracker_CLASS/c++/src/trackutils.cpp
+                        tracker_CLASS/c++/src/plot.cpp
+                        tracker_CLASS/c++/src/tracker.cpp )
+
+                        
+target_link_libraries(yolo3_demo tkDNN CLASS)
+
+
 
 
 #-------------------------------------------------------------------------------

README.md

@@ -8,6 +8,12 @@ this branch actually work on every NVIDIA GPU that support the dependencies:
 * TENSORRT 6.01
 * OPENCV 4.1
 
+## Dependencies
+
+```
+sudo apt install libgdal-dev libeigen3-dev python-matplotlib libyaml-cpp-dev libcereal-dev python2.7-dev
+```
+
 ## Workflow
 The recommended workflow follow these step:
 * Build and train a model in Keras (on any PC)
@@ -49,3 +55,4 @@ this will genereate a yolo3_berkeley.rt file that can be used for live detection
 ./yolo3_demo                               # launch detection on a demo video
 ./yolo3_demo yolo3_berkeley.rt /dev/video0 # launch detection on device 0
 ```
+![demo](https://user-images.githubusercontent.com/11562617/72547657-540e7800-388d-11ea-83c6-49dfea2a0607.gif)

demo/demo/data/calib36.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Mon 06 May 2019 11:32:13 PM CEST"
+image_width: 1920
+image_height: 1080
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.6158690952190570e+03, 0., 9.4702812371722337e+02, 0.,
+       1.6123979985757153e+03, 5.1995630055718266e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -4.0971199964304100e-01, 1.8755404192050384e-01,
+       -5.3059322427743867e-03, -1.0380603625304912e-03, 0. ]
+avg_reprojection_error: 3.4351035832972515e-01

demo/demo/data/calib_cameras/10218.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:32:53 AM CEST"
+image_width: 3072
+image_height: 1728
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 4.7264390181579711e+03, 0., 1.5059850098280642e+03, 0.,
+       4.6793092340700096e+03, 6.7300681982359868e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -4.2669569210605879e-01, 6.6337608795749903e-01,
+       -1.3881256269106437e-03, 5.2468063845700682e-03, 0. ]
+avg_reprojection_error: 3.1312290189919406e-01

demo/demo/data/calib_cameras/20932.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:14:44 AM CEST"
+image_width: 1920
+image_height: 1080
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.6902498656747011e+03, 0., 9.7959318966703324e+02, 0.,
+       1.7552884617253583e+03, 5.3327953707582492e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -5.4891909767312119e-01, 2.5555919841568631e-01,
+       -4.3831358875660656e-03, -1.3934378903760349e-02, 0. ]
+avg_reprojection_error: 1.1758482932800183e+00

demo/demo/data/calib_cameras/20936.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Mon 06 May 2019 11:32:13 PM CEST"
+image_width: 1920
+image_height: 1080
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.6158690952190570e+03, 0., 9.4702812371722337e+02, 0.,
+       1.6123979985757153e+03, 5.1995630055718266e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -4.0971199964304100e-01, 1.8755404192050384e-01,
+       -5.3059322427743867e-03, -1.0380603625304912e-03, 0. ]
+avg_reprojection_error: 3.4351035832972515e-01

demo/demo/data/calib_cameras/20937.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 09:56:50 AM CEST"
+image_width: 1920
+image_height: 1080
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.6229477302581809e+03, 0., 1.0277357980566628e+03, 0.,
+       1.6485741394129034e+03, 5.5596919291027621e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -3.7853584845653426e-01, 7.8553352913896368e-02,
+       -6.5552938633907229e-03, -1.6436824648695104e-02, 0. ]
+avg_reprojection_error: 8.4629096638637347e-01

demo/demo/data/calib_cameras/20939.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Fri 03 May 2019 11:56:13 PM CEST"
+image_width: 3072
+image_height: 1728
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 5.8796921906556563e+03, 0., 1.3036708932691290e+03, 0.,
+       5.9435402023228071e+03, 8.1110067822514861e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -5.4688862790206871e-01, 5.1913397860290666e-01,
+       -2.1076612628273591e-03, 1.6869796115416984e-02, 0. ]
+avg_reprojection_error: 6.7667474319420251e-01

demo/demo/data/calib_cameras/20940.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Sat 04 May 2019 12:00:38 AM CEST"
+image_width: 3072
+image_height: 1728
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 4.6903033136815602e+03, 0., 1.6303445000881884e+03, 0.,
+       4.7582671272189546e+03, 4.3596515032334111e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -3.4366857232996317e-01, 2.2799325522263861e-01,
+       2.0765840315530557e-02, -4.0088654509745098e-03, 0. ]
+avg_reprojection_error: 3.9811872397860709e-01

demo/demo/data/calib_cameras/6310.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:20:53 AM CEST"
+image_width: 3072
+image_height: 1728
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 2.5005410461483498e+03, 0., 1.5319405824251596e+03, 0.,
+       2.5001544574623872e+03, 7.8267345299919543e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -3.7379752112038928e-01, 1.6246299444310250e-01,
+       8.0371978716752837e-04, -9.6108499236087584e-04, 0. ]
+avg_reprojection_error: 3.6334262234685299e-01

demo/demo/data/calib_cameras/6311.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:37:47 AM CEST"
+image_width: 960
+image_height: 720
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 5.0439587680799593e+02, 0., 4.8997081391816727e+02, 0.,
+       5.0714582349015507e+02, 3.5481348085748095e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -2.7916140864065331e-01, 6.5465070220501562e-02,
+       -1.9231901334709591e-03, -2.6191562264760264e-03, 0. ]
+avg_reprojection_error: 5.7283635087126605e-01

demo/demo/data/calib_cameras/6312.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:41:12 AM CEST"
+image_width: 960
+image_height: 720
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 5.1663651913150818e+02, 0., 4.7267297458218127e+02, 0.,
+       5.1291090124818436e+02, 3.8505850298928243e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -2.8051872523046845e-01, 6.0895269981008610e-02,
+       -9.7920840355269542e-03, -4.9804820350633240e-04, 0. ]
+avg_reprojection_error: 5.4967308787122626e-01

demo/demo/data/calib_cameras/6313.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:50:02 AM CEST"
+image_width: 960
+image_height: 720
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 4.9724079419911664e+02, 0., 4.9277930193807083e+02, 0.,
+       4.9700744926387819e+02, 3.6581239154403062e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -2.7582961261093608e-01, 6.6908017283259263e-02,
+       -2.1580546593114500e-03, -1.7921711595441153e-03, 0. ]
+avg_reprojection_error: 3.7129088933918375e-01

demo/demo/data/calib_cameras/6314.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:53:27 AM CEST"
+image_width: 960
+image_height: 720
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 4.9372152821507876e+02, 0., 4.7585791077351445e+02, 0.,
+       4.9644139996881893e+02, 3.5961856724726260e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -2.9023109325424973e-01, 8.3150964750672046e-02,
+       -6.1378621304345154e-04, 8.4481910933416999e-04, 0. ]
+avg_reprojection_error: 3.4691001942524069e-01

demo/demo/data/calib_cameras/6315.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Sat 04 May 2019 12:35:58 AM CEST"
+image_width: 3840
+image_height: 2160
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 2.1723071272381276e+03, 0., 1.9718118689531000e+03, 0.,
+       2.2377541672328439e+03, 9.3157209524899565e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -3.8516162509048857e-01, 1.8961757063227327e-01,
+       1.8297248985443184e-02, -8.9166274086698288e-03, 0. ]
+avg_reprojection_error: 9.9886914863900311e-01

demo/demo/data/calib_cameras/632.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Sun 05 May 2019 08:52:13 PM CEST"
+image_width: 3072
+image_height: 1728
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 2.9841357325808735e+03, 0., 1.5379802472694901e+03, 0.,
+       2.9784613885271938e+03, 8.9330228722164566e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -5.1096356919758967e-01, 1.4543132746407733e-01,
+       -3.1254001577433334e-02, -1.4769334036191385e-02, 0. ]
+avg_reprojection_error: 9.3544537534095662e-01

demo/demo/data/calib_cameras/634.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Sun 05 May 2019 09:24:33 PM CEST"
+image_width: 3072
+image_height: 1728
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 2.4796388675592771e+03, 0., 1.5358283835422017e+03, 0.,
+       2.4440198814655632e+03, 8.9911455540136217e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -3.9478938399374452e-01, 1.6288159087710818e-01,
+       -1.8565610712959927e-02, -7.0112574756757643e-03, 0. ]
+avg_reprojection_error: 4.5805459213906724e-01

demo/demo/data/calib_cameras/637.params

@@ -0,0 +1,22 @@
+%YAML:1.0
+---
+calibration_time: "Tue 07 May 2019 10:03:44 AM CEST"
+image_width: 3072
+image_height: 1728
+board_width: 8
+board_height: 6
+square_size: 2.4799999237060547e+01
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 2.6027348174982544e+03, 0., 1.4808496083807213e+03, 0.,
+       2.6008830910556521e+03, 6.7577068120137187e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -3.4912899377320661e-01, 1.5704840296202566e-01,
+       6.4926875404798358e-03, 5.7293259996249049e-03, 0. ]
+avg_reprojection_error: 4.0040122960491076e-01

demo/demo/data/pmat_geo/pmat_geo_10218.txt

@@ -0,0 +1,3 @@
+-7.327951322519857 -89.11104695972499 9201.012307315486 
+-3.2739347567332553 -42.14373356189513 3668.5149400959867 
+-0.0009310035308911926 -0.010684335913469737 0.9999999999999999 

demo/demo/data/pmat_geo/pmat_geo_20932.txt

@@ -0,0 +1,3 @@
+-5.575118981764042 19.15613757077127 7116.209593619794 
+-3.2016502214672586 11.087267568165238 4660.384266971425 
+-0.0007553414127200226 0.002676057142671632 1.0 

demo/demo/data/pmat_geo/pmat_geo_20936.txt

@@ -0,0 +1,3 @@
+-0.6209787370656474 -57.192772931734694 6553.189887121747 
+-0.655304496315561 -28.195100762686465 3423.351597211246 
+-0.0001275987755308679 -0.007884524572727637 1.0 

demo/demo/data/pmat_geo/pmat_geo_20937.txt

@@ -0,0 +1,3 @@
+21.441694474392808 72.45500438933801 4652.4523150318 
+9.195810184174585 36.76655098601718 1801.6321216143722 
+0.0027471614789484795 0.009610371445379061 1.0 

demo/demo/data/pmat_geo/pmat_geo_20939.txt

@@ -0,0 +1,3 @@
+23.89713091621312 60.523880117656084 7778.591483347838 
+11.708054761838232 41.819183783017614 1126.9348661897782 
+0.0029807026524197224 0.008783281479831609 0.9999999999999999 

demo/demo/data/pmat_geo/pmat_geo_20940.txt

@@ -0,0 +1,3 @@
+-2.986713098145805 -21.641128011999868 6870.903103366085 
+-2.2937864581163416 -14.410613254705751 4594.877638262097 
+-0.0004752252182089383 -0.003112785793525571 1.0 

demo/demo/data/pmat_geo/pmat_geo_6310.txt

@@ -0,0 +1,3 @@
+0.797119441300711 35.495698816749574 2225.384878217832 
+0.3510443326339413 59.809425152982726 1419.4212456173686 
+-1.8197506232485688e-05 0.01820976131616732 1.0 

demo/demo/data/pmat_geo/pmat_geo_6311.txt

@@ -0,0 +1,3 @@
+2.213864892663308 -21.831117643802187 4060.61306610222 
+0.9075230321117119 -7.525265027425343 1268.217122301812 
+0.0005431549026467866 -0.0054930502425022745 1.0 

demo/demo/data/pmat_geo/pmat_geo_6312.txt

@@ -0,0 +1,3 @@
+-8.65061527352736 -23.247834477892475 4009.9545358742134 
+-2.2093309523396223 -8.398154406654037 1529.4606465341913 
+-0.001952580524058866 -0.006089308376531831 1.0 

demo/demo/data/pmat_geo/pmat_geo_6313.txt

@@ -0,0 +1,3 @@
+-8.665173353556622 -19.056833294770822 3827.027102449301 
+-2.728678410673174 -6.695956964089541 1523.1971944608492 
+-0.0020218867007343213 -0.004877696027397352 1.0 

demo/demo/data/pmat_geo/pmat_geo_6314.txt

@@ -0,0 +1,3 @@
+-0.5624066154780419 -26.36968028701744 3384.7920236393156 
+-0.6590389521668137 -8.709883019993336 1162.7049573736972 
+-0.00023421305979212285 -0.006824826866955653 0.9999999999999999 

demo/demo/data/pmat_geo/pmat_geo_6315.txt

@@ -0,0 +1,3 @@
+-0.9691844827251948 -16.63913290283819 3991.384826153086 
+-0.47672518258283236 -11.365071723693541 1770.7201067235435 
+-0.0003215600914247826 -0.005142032883105236 1.0 

demo/demo/data/pmat_geo/pmat_geo_634.txt

@@ -0,0 +1,3 @@
+-1.5172087045737013 5.226213011053854 2493.822558690447 
+-1.4892268949636678 4.265359557115566 3382.185465939972 
+-0.000492856899218649 0.0016917382715537009 1.0 

demo/demo/data/pmat_geo/pmat_geo_636.txt

@@ -0,0 +1,3 @@
+3.671630456405992 102.56885593938215 3570.381264275829 
+1.5985035927893978 46.57491080817888 -1491.4874345505793 
+0.0006446393888135914 0.027894339637393125 1.0 

demo/demo/data/pmat_geo/pmat_geo_637.txt

@@ -0,0 +1,3 @@
+0.44666021794890903 -32.932455985363575 3557.76647014547 
+1.0444994361599629 -37.23760709667856 4404.4517853552425 
+0.00029114742043039743 -0.010721230871024154 1.0 

demo/demo/data/pmundist.txt

@@ -0,0 +1,3 @@
+0.1982518156851575	-13.22038303683631	595.3838470607648	
+-0.4806350794284377	-6.567788488139482	598.1732743332492	
+-0.00016595895699412456	-0.007499178419813009	1.0

demo/demo/data/proj_matrix_map_b.txt

@@ -0,0 +1,3 @@
+-0.4122368700442484 -10.479982981650977 812.1821012303 
+-0.6536846365005352 -4.951476039607947 512.0831604767536 
+-0.00042630219831388434 -0.006003223898658603 1.0 

demo/demo/demo.cpp

@@ -1,109 +1,603 @@
-#include <iostream>
+#include <time.h>
-#include <signal.h>
+
-#include <stdlib.h>     /* srand, rand */
-#include <unistd.h>
-#include <mutex>
 #include "utils.h"
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/videoio.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
 #include "Yolo3Detection.h"
+#include "message.h"
+#include "visualization.h"
+#include "configuration.h"
+
+#include "tracker.h"
+#include "../masa_protocol/include/send.hpp"
+#include "../masa_protocol/include/serialize.hpp"
+
+// #include <assert.h>
+// #include <unistd.h>
+// #include <mutex>
+// #include <ctime>
+// #include <pthread.h>
+// #include <signal.h>
+// #include <chrono>
+// #include <math.h>
+// #include <typeinfo>
+// #include <iostream>
+
+#define MAX_DETECT_SIZE 100
 
 bool gRun;
+std::chrono::steady_clock::time_point local_clock_start;
+std::mutex mutexgRun;
+std::string obj_class[10]{"person", "car", "truck", "bus", "motor", "bike", "rider", "traffic light", "traffic sign", "train"};
+//mutex for some opencv operations
+std::mutex mutex_cv;
+Show_t updates;
 bool SAVE_RESULT = false;
 
-void sig_handler(int signo) {
+void sig_handler(int signo)
+{
     std::cout << "request gateway stop\n";
+    mutexgRun.lock();
     gRun = false;
+    mutexgRun.unlock();
 }
 
-int main(int argc, char *argv[]) {
+void *readVideoCapture(void *x_void_ptr)
+{
+    std::cout << "readVideoCapture start...\n";
 
-    std::cout<<"detection\n";
+    Frame_t *info_f = (Frame_t *)x_void_ptr;
-    signal(SIGINT, sig_handler);
+    mutex_cv.lock();
-
+    cv::VideoCapture cap(info_f->input, cv::CAP_FFMPEG);
-
+    mutex_cv.unlock();
-    char *net = "yolo3_berkeley.rt";
+    cv::Mat frame_loc, frame0;
-    if(argc > 1)
+    int frame_nbr_loc = 0;
-        net = argv[1]; 
+    // bool to_show = false;
-    char *input = "../demo/yolo_test.mp4";
-    if(argc > 2)
-        input = argv[2]; 
-
-    tk::dnn::Yolo3Detection yolo;
-    yolo.init(net);
-
-    gRun = true;
-
-    cv::VideoCapture cap(input);
     if (!cap.isOpened())
+    {
+        mutexgRun.lock();
         gRun = false;
+        mutexgRun.unlock();
+    }
     else
         std::cout << "camera started\n";
 
+    // 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();
+    auto current_timestamp = std::chrono::steady_clock::now();
 
-    cv::VideoWriter resultVideo;
+    // compute fps and find camera's clock
-    if(SAVE_RESULT) {
+    double shift, mean_time = 0;
-        int w = cap.get(cv::CAP_PROP_FRAME_WIDTH);
+    std::cout << "Frames per second using video.get(cv::CAP_PROP_FPS) : " << cap.get(cv::CAP_PROP_FPS) << std::endl;
-        int h = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
+    std::cout << "readVideoCapture computes frame rate...\n";
-        resultVideo.open("result.mp4", cv::VideoWriter::fourcc('M','P','4','V'), 30, cv::Size(w, h));
+    // //compute frame rate
+    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;
+
+        if (!frame_loc.data)
+        {
+            usleep(1000000);
+            mutex_cv.lock();
+            cap.open(info_f->input);
+            printf("cap reinitialize\n");
+            mutex_cv.unlock();
+            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;
+
+        info_f->sem_vc.lock();
+        info_f->frame = frame_loc.clone();
+        info_f->frame_nbr = frame_nbr_loc;
+        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;
+}
+
+void *computationTask(void *x_void_ptr)
+{
+    Camera_t *camera = (Camera_t *)x_void_ptr;
+    pthread_t visual, originalshow, detectionshow, topviewshow, disparityshow;
+    pthread_t videocap;
+    tk::dnn::Yolo3Detection yolo = *(camera->yolo);
+    //create video capture thread
+    Frame_t info_f;
+    info_f.input = camera->input;
+    if (pthread_create(&videocap, NULL, readVideoCapture, (void *)&info_f))
+    {
+        fprintf(stderr, "Error creating thread\n");
+        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;
+        };
+    }
+    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(camera->cameraCalib, cameraMat, distCoeff);
+    std::cout << cameraMat << std::endl;
+    std::cout << distCoeff << std::endl;
+
+    /*GPS information*/
+    std::vector<ObjCoords> coords;
+
+    /*socket*/
+    Communicator Comm(SOCK_DGRAM);
+    Comm.open_client_socket((char *)"127.0.0.1", 8888);
+
+    Message *m = new Message;
+    m->cam_idx = camera->CAM_IDX;
+    m->lights.clear();
+    /*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;
+    /*Mask info*/
+    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++)
+    {
+        for(int j=0; j< mask.rows; j++)
+        {
+            std::cout<<maskOrient.at<cv::Vec3b>(i,j)    <<std::endl;
+        }
+    }
+    
+   
+    return 0;*/
+    /*tracker infos*/
+    std::vector<Tracker> trackers;
+    std::vector<Data> cur_frame;
+    int initial_age = -5;
+    int age_threshold = -8;
+    int n_states = 5;
+    float dt = 0.03;
+
+    int frame_nbr = 0;
+
+    //save video
+    /*cv::VideoWriter outputVideo;
+    cv::Size S = cv::Size((int)cap.get(cv::CAP_PROP_FRAME_WIDTH), //Acquire input size
+                          (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;
+    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();
+
+    //TODO: move in a thread
+    // // information for the disparity map
+    // std::vector <cv::Rect> pre_rois;
+    // cv::Mat pre_frame;
+    cv::Mat orig_frame;
+    // cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
+    // cv::Mat canny_img;
+
+    // box variable
+    tk::dnn::box b;
+    int x0, h, y1; //w, x1, y0;
+    int objClass;
+    std::string det_class;
+    ;
+    // float prob;
+    cv::Scalar intensity;
+
+
+    // cv::VideoWriter resultVideo;
+    // if(SAVE_RESULT) {
+    //     int w = cap.get(cv::CAP_PROP_FRAME_WIDTH);
+    //     int h = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
+    //     resultVideo.open("result.mp4", cv::VideoWriter::fourcc('M','P','4','V'), 30, cv::Size(w, h));
+    // }
+
     cv::Mat frame;
+    cv::Mat frame_crop;
     cv::Mat dnn_input;
-    cv::namedWindow("detection", cv::WINDOW_NORMAL);
+    bool first_iteration = true;
 
-    while(gRun) {
+    while (gRun)
-        cap >> frame; 
+    {
-        if(!frame.data) {
+        TIMER_START
-            break;
+        start_t = std::chrono::steady_clock::now();
+        step_t = start_t;
+
+        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 (diff " << info_f.frame_nbr - frame_nbr << ")\n";
+        frame_nbr = info_f.frame_nbr;
+        info_f.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();
+        // 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();
+        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
-        for(int i=0; i<yolo.detected.size(); i++) {
+        std::cout << "camera: " << camera->CAM_IDX << " - num detected: " << num_detected << std::endl;
-            tk::dnn::box b = yolo.detected[i];
-            int x0   				= b.x;
-            int x1   				= b.x + b.w;
-            int y0   				= b.y;
-            int y1   				= b.y + b.h;
-            std::string det_class 	= yolo.getYoloLayer()->classesNames[b.cl];
-            float prob 				= b.prob;
 
-            std::cout<<det_class<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
+        //TODO: move in a thread
-			// draw rectangle
+        // //preprocessing frame
-            cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[b.cl], 2); 
+        // 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::COLOR_GRAY2RGB);
+        //     cv::cvtColor(canny, canny_RGB, cv::COLOR_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;
 
-	        // draw label
+        //     // //--------------------------------
-            int baseline = 0;
+        //     // //frame box disparity on the original image
-            float fontScale = 0.5;
+        //     // step_t_segmentation = std::chrono::steady_clock::now();
-            int thickness = 2;
+        //     // frame_box_disparity(pre_frame, frame, pre_rois, frame_nbr);
-            cv::Size textSize = getTextSize(det_class, cv::FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
+        //     // // reset pre_rois for the new roi of the current frame
-            cv::rectangle(frame, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), yolo.colors[b.cl], -1);                      
+        //     // // pre_rois.erase(pre_rois.begin(), pre_rois.end());
-            cv::putText(frame, det_class, cv::Point(x0, (y0 - (baseline / 2))), cv::FONT_HERSHEY_SIMPLEX, fontScale, cv::Scalar(255, 255, 255), thickness);
+        //     // 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::COLOR_GRAY2RGB);
+        //     // cv::cvtColor(canny, canny_RGB, cv::COLOR_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);
+
+        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])
+            {
+
+                if (objClass < 6)
+                {
+
+                    // 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;
+                    // 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);
+
+                    // segmentation(frame(roi), frame(roi), frame_nbr, i, 1);
+
+                    /////
+                    convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform);
+
+                    // //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
+                    // 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)), 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);
+                }
+            }
         }
 
-        cv::imshow("detection", frame);
+        end_t = std::chrono::steady_clock::now();
-        cv::waitKey(1);
+        std::cout << " TIME 2 : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms" << std::endl;
-        if(SAVE_RESULT)
+        step_t = end_t;
-            resultVideo << frame;
+        //convert from latitude and longitude to meters for ekf
+        cur_frame.clear();
+        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_));
+        }
+        if (first_iteration)
+        {
+            // if there aren't detections and it is the first iteration, we can't initialize the tracker, so continue
+            if (cur_frame.empty())
+                continue;
+            for (auto f : cur_frame)
+                trackers.push_back(Tracker(f, initial_age, dt, n_states));
+        }
+        else
+        {
+            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
+        if (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
+            if (!m->objects.empty())
+                Comm.send_message(m);
         }
 
+        if (to_show)
+        {
+            //populate the ModFrame_t
+            info_show.sem.lock();
+            info_show.original_frame = frame.clone();
+            // std::vector<Tracker> trackers;
+            info_show.trackers = trackers;
+            // geodetic_converter::GeodeticConverter gc;
+            info_show.gc = gc;
+            for (int i = 0; i < 6; i++)
+                info_show.adfGeoTransform[i] = adfGeoTransform[i];
+            // cv::Mat H;
+            info_show.H = H.clone();
+            info_show.yolo = yolo;
+            // std::copy(camera->yolo.begin(), camera->yolo.end(), info_show.yolo.begin());
+            info_show.mask = mask.clone();
+            info_show.sem.unlock();
+        }
+
+        // update pre_frame for the disparity map
+        // pre_frame = orig_frame.clone();
+        // pre_canny = canny.clone();
+        if (first_iteration)
+            first_iteration = false;
+
+        frame_nbr++;
+        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));
+
+    Parameters_t par;
+
+    if(!read_parameters(argc, argv, &par))
+        return -1;
+    
+    tk::dnn::Yolo3Detection yolo[par.n_cameras];
+    for(int i=0; i<par.n_cameras; i++)
+    {
+        yolo[i].init(par.net);
+        yolo[i].thresh = 0.25;    
+        
+        // if(SAVE_RESULT)
+        //     resultVideo << frame;
+    }
+    // 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(par.tiffile, adfGeoTransform);
+    // Camera_t cameras[par.n_cameras]; 
+    for(int i=0; i<par.n_cameras; i++)
+    {
+        for(int j = 0; j < 6; j++ )
+            par.cameras[i].adfGeoTransform[j] = adfGeoTransform[j];
+        par.cameras[i].yolo = &yolo[i];
+        // par.cameras[i].yolo.init(par.net);
+        // par.cameras[i].yolo.thresh = 0.25;
+        // cameras[i].yolo = yolo[i];
+        // cameras[i].yolo = yolo;
+        
+    }
+    pthread_t camera_task[par.n_cameras];
+    for(int i=0; i<par.n_cameras; i++)
+    {
+        std::cout<<"creating thread\n";
+        if(pthread_create(&camera_task[i], NULL, computationTask, (void*)&(par.cameras[i])))
+        {
+            fprintf(stderr, "error creating thread\n");
+            return 1;
+        }
+    }
+    for(int i=0; i<par.n_cameras; i++)
+    {
+        pthread_join(camera_task[i], NULL);
+    }
+    std::cout <<" free adfGeoT \n";
+    free(adfGeoTransform);
     std::cout << "detection end\n";
-
-
-    std::cout<<COL_GREENB<<"\n\nTime stats:\n";
-    std::cout<<"Min: "<<*std::min_element(yolo.stats.begin(), yolo.stats.end())<<" ms\n";    
-    std::cout<<"Max: "<<*std::max_element(yolo.stats.begin(), yolo.stats.end())<<" ms\n";    
-    double mean = 0; for(int i=0; i<yolo.stats.size(); i++) mean += yolo.stats[i]; mean /= yolo.stats.size();
-    std::cout<<"Avg: "<<mean<<" ms\n"<<COL_END;    
     return 0;
 }
-

include/boxDetection.h

@@ -0,0 +1,44 @@
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <iomanip>
+#include <stdlib.h>
+#include <cstring>
+#include <cstdlib>
+#include <time.h>
+#include <chrono>
+
+#include "cuda.h"
+#include "cuda_runtime_api.h"
+#include <cublas_v2.h>
+#include <cudnn.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+//saliency
+#include <opencv2/core/utility.hpp>
+//#include <opencv2/saliency.hpp>
+#include <opencv2/highgui.hpp>
+
+#define SAVE false
+#define SAVE_TO(name, fn, i, var) {sprintf(buf_frame_crop_name,name,fn,i);\
+    cv::imwrite(buf_frame_crop_name, var);}
+
+
+
+// cv::Mat img_threshold(cv::Mat frame_crop);
+// cv::Mat img_background(cv::Mat frame_crop);
+// cv::Mat img_dist_transform(cv::Mat frame_crop);
+// cv::Mat img_watershed(cv::Mat frame_crop);
+void image_segmentation(cv::Mat frame_crop, int frame_nbr, int i);
+void image_gradients(cv::Mat frame_crop, int frame_nbr, int i);
+void image_find_contours(cv::Mat frame_crop, int frame_nbr, int i);
+void image_saliency(cv::Mat frame_crop, int frame_nbr, int i);
+void frame_box_disparity(cv::Mat pre_frame, cv::Mat frame, std::vector <cv::Rect>  pre_rois, int frame_nbr);
+void segmentation(cv::Mat pre_frame, cv::Mat frame_crop, int frame_nbr, int i, int mode);
+
+//canny
+cv::Mat img_laplacian(cv::Mat frame_crop, int ret);
+cv::Mat frame_disparity(cv::Mat pre_frame, cv::Mat frame, int frame_nbr, int i, int ret);

include/calibration.h

@@ -0,0 +1,32 @@
+#ifndef CALIBRATION_H
+#define CALIBRATION_H
+
+#include "gdal.h"
+#include <gdal_priv.h>
+#include <gdal/gdal.h>
+#include "gdal/gdal_priv.h"
+#include "gdal/cpl_conv.h"
+
+#include <yaml-cpp/yaml.h>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+
+#include <iostream>
+#include <cstring>
+
+struct ObjCoords
+{
+    double lat_;
+    double long_;
+    int class_;
+};
+
+void readTiff(char *filename, double *adfGeoTransform);
+void readCameraCalibrationYaml(const std::string &cameraCalib, cv::Mat &cameraMat, cv::Mat &distCoeff);
+void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform);
+void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform);
+void fillMatrix(cv::Mat &H, double *matrix, bool show = false);
+void read_projection_matrix(cv::Mat &H, char *path);
+void convert_coords(std::vector<ObjCoords> &coords, int x, int y, int detected_class, cv::Mat H, double *adfGeoTransform);
+
+#endif /*CALIBRATION_H*/

include/cameraUtils.h

@@ -0,0 +1,45 @@
+#ifndef CAMERAUTILS_H
+#define CAMERAUTILS_H
+
+#include <vector>
+#include <mutex>
+#include <opencv2/core/core.hpp>
+#include "tracker.h"
+#include "Yolo3Detection.h"
+
+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 Frame_t
+{
+    char *input;
+    cv::Mat frame;
+    int frame_nbr;
+    // sem_vc for mainthread, videocapturethread, originalthread and disparitythread
+    std::mutex sem_vc;
+};
+
+struct ModFrame_t
+{
+    std::vector<Tracker> trackers;
+    geodetic_converter::GeodeticConverter gc;
+    double adfGeoTransform[6];
+    cv::Mat H;
+    cv::Mat original_frame;
+    tk::dnn::Yolo3Detection yolo;
+    cv::Mat mask;
+    // sem for mainthread, detectionthread and topviewthread
+    std::mutex sem;
+};
+
+#endif /*CAMERAUTILS_H*/

include/configuration.h

@@ -0,0 +1,21 @@
+#ifndef CONFIGURATION_H
+#define CONFIGURATION_H
+
+#include "cameraUtils.h"
+#include <iostream>
+#include <cstring>
+
+#include <yaml-cpp/yaml.h>
+
+struct Parameters_t 
+{
+    char *net;
+    char *tiffile;
+    int n_cameras;
+    Camera_t *cameras;
+};
+
+void readCamerasParametersYaml(const std::string &camerasParams, Parameters_t *par);
+bool read_parameters(int argc, char *argv[], Parameters_t *par);
+
+#endif /*CONFIGURATION_H*/

include/message.h

@@ -0,0 +1,22 @@
+#ifndef MESSAGE_H
+#define MESSAGE_H
+
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+// #include <sys/socket.h> //socket
+// #include <arpa/inet.h>  //inet_addr
+// #include <unistd.h>     //write
+
+#include "tracker.h"
+
+#include "../masa_protocol/include/send.hpp"
+#include "../masa_protocol/include/serialize.hpp"
+
+unsigned long long time_in_ms();
+
+void addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, geodetic_converter::GeodeticConverter &gc, const cv::Mat &maskOrient, double *adfGeoTransform, cv::Mat H);
+
+#endif /*MESSAGE_H*/

include/tkDNN/Layer.h

@@ -6,11 +6,16 @@
 #include "utils.h"
 #include "Network.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
 enum layerType_t {
+    LAYER_INPUT,
     LAYER_DENSE,
     LAYER_CONV2D,
+    LAYER_LSTM,
     LAYER_ACTIVATION,
     LAYER_FLATTEN,
     LAYER_MULADD,
@@ -29,14 +34,16 @@ enum layerType_t {
 /** 
     Simple layer Father class
 */
-class Layer {
+class Layer
+{
 
 public:
     Layer(Network *net);
     virtual ~Layer();
     virtual layerType_t getLayerType() = 0;
 
-    virtual dnnType* infer(dataDim_t &dim, dnnType* srcData) {
+    virtual dnnType *infer(dataDim_t &dim, dnnType *srcData)
+    {
         std::cout << "No infer action for this layer\n";
         return NULL;
     }
@@ -44,11 +51,14 @@ public:
     dataDim_t input_dim, output_dim;
     dnnType *dstData; //where results will be putted
 
-    std::string getLayerName() {
+    std::string getLayerName()
+    {
         layerType_t type = getLayerType();
         switch(type) {
+            case LAYER_INPUT:       return "Input";
             case LAYER_DENSE:       return "Dense";
             case LAYER_CONV2D:      return "Conv2d";
+            case LAYER_LSTM:        return "LSTM";
             case LAYER_ACTIVATION:  return "Activation";
             case LAYER_FLATTEN:     return "Flatten";
             case LAYER_MULADD:      return "MulAdd";
@@ -67,14 +77,13 @@ public:
 protected:
     Network *net;
     cudnnTensorDescriptor_t srcTensorDesc, dstTensorDesc;
-
 };
 
-
 /**
     Father class of all layer that need to load trained weights
 */
-class LayerWgs : public Layer {
+class LayerWgs : public Layer
+{
 
 public:
     LayerWgs(Network *net, int inputs, int outputs, int kh, int kw, int kt, 
@@ -104,11 +113,32 @@ public:
     __half *variance16_h, *variance16_d;
 };
 
+/**
+    Input layer (it doesnt need weigths)
+*/
+class Input : public Layer {
+
+public:
+
+    Input(Network *net, dataDim_t &dim, dnnType* srcData) : Layer(net) {
+        input_dim = dim;
+        output_dim = dim;
+        dstData = srcData;
+    }
+    virtual ~Input() {}
+    virtual layerType_t getLayerType() { return LAYER_INPUT; };
+
+    virtual dnnType* infer(dataDim_t &dim, dnnType* srcData) {
+        return dstData;
+    }
+};
+
 
 /**
     Dense (full interconnection) layer
 */
-class Dense : public LayerWgs {
+class Dense : public LayerWgs
+{
 
 public:
     Dense(Network *net, int out_ch, std::string fname_weights); 
@@ -118,11 +148,11 @@ public:
     virtual dnnType *infer(dataDim_t &dim, dnnType *srcData);
 };
 
-
 /**
     Avaible activation functions
 */
-typedef enum {
+typedef enum
+{
     ACTIVATION_ELU = 100,
     ACTIVATION_LEAKY = 101
 } tkdnnActivationMode_t;
@@ -130,7 +160,8 @@ typedef enum {
 /**
     Activation layer (it doesnt need weigths)
 */
-class Activation : public Layer {
+class Activation : public Layer
+{
 
 public:
     int act_mode;
@@ -145,11 +176,19 @@ protected:
     cudnnActivationDescriptor_t activDesc;
 };
 
-
 /**
     Convolutional 2D layer
+
+    WEIGHTS shape: OUTCH, INCH, KH, KW ...
+    BIAS shape: OUTCH
+
+    with BATCHNORM:
+        scales:   OUTCH
+        means:    OUTCH
+        variance: OUTCH
 */
-class Conv2d : public LayerWgs {
+class Conv2d : public LayerWgs
+{
 
 public:
     Conv2d( Network *net, int out_ch, int kernelH, int kernelW, 
@@ -172,12 +211,78 @@ protected:
     size_t ws_sizeInBytes;
 };
 
+/**
+    Bidirectional LSTM layer
+    ONLY BIDIRECTIONAL (TODO: more configurable)
+    currently implemented as 2 inferences: forward and backward (TODO: only 1 cudnn inference)
+
+    implementation info:
+    https://github.com/jiangnanhugo/seq2seq_cuda/blob/e4dbdcfa0517c972bfd4beea9f11a5233954093c/src/rnn.cpp
+    https://github.com/Jeffery-Song/mxnet-test/blob/aab666faad44011f7a67b527b5f6c960367d0422/src/operator/cudnn_rnn-inl.h
+    https://stackoverflow.com/a/38737941
+    https://colah.github.io/posts/2015-08-Understanding-LSTMs/
+
+    PARAMS (numlayers*2):
+        layer0:
+            ( INCH, ? )    ???
+            ( HIDDEN, ? )  ???
+            ( HIDDEN * 8 ) ???
+        layer2:
+            ( INCH, ? )    ???
+            ( HIDDEN, ? )  ???
+            ( HIDDEN * 8 ) ???
+
+    OUTPUT shape:
+        (N, C, 1, W) ---> LSTM(HIDDEN, returnSeq=True)  ---> (N, 2*HIDDEN, 1, W)   # W is seqLength 
+        (N, C, 1, W) ---> LSTM(HIDDEN, returnSeq=False) ---> (N, 2*HIDDEN, 1, 1)
+*/
+class LSTM : public Layer {
+
+public:
+    LSTM(Network *net, int hiddensize, bool returnSeq, std::string fname_weights);
+    virtual ~LSTM();
+    virtual layerType_t getLayerType() { return LAYER_LSTM; };
+
+    virtual dnnType* infer(dataDim_t &dim, dnnType* srcData);
+
+    const bool bidirectional = true; /**> is the net bidir */
+    bool returnSeq = false;       /**> if false return only the result of last timestep */
+    int stateSize = 0; /**> number of hidden states */
+    int seqLen = 0;    /**> number of timesteps */
+    int numLayers = 1; /**> number of internal layers */
+
+protected:
+    cudnnRNNDescriptor_t rnnDesc;
+    cudnnDropoutDescriptor_t dropoutDesc;
+    dnnType  *dropout_states_, *work_space_;
+
+    size_t workspace_byte_, dropout_byte_;
+    int workspace_size_, dropout_size_;
+    
+    std::vector<cudnnTensorDescriptor_t> x_desc_vec_, y_desc_vec_;
+    cudnnTensorDescriptor_t hx_desc_, cx_desc_;
+    cudnnTensorDescriptor_t hy_desc_, cy_desc_;
+    dnnType *hx_ptr, *cx_ptr, *hy_ptr, *cy_ptr;
+    int stateDataDim;
+
+    cudnnFilterDescriptor_t w_desc_;
+    dnnType *w_ptr;
+    dnnType *w_h;
+    dnnType *wf_ptr, *wb_ptr; // params pointer forward and backward layer
+
+    // used during inference
+    dataDim_t one_output_dim; // output dim of as single inference
+    dnnType *srcF, *srcB; // input of single inference 
+    dnnType *dstF, *dstB_NR, *dstB; // output of single inference, dstB_NR = dstB not reversed
+};
+
 
 /**
     Flatten layer
     is actually a matrix transposition
 */
-class Flatten : public Layer {
+class Flatten : public Layer
+{
 
 public:
     Flatten(Network *net);
@@ -187,12 +292,12 @@ public:
     virtual dnnType *infer(dataDim_t &dim, dnnType *srcData);
 };
 
-
 /**
     MulAdd layer
     apply a multiplication and then an addition for each data
 */
-class MulAdd : public Layer {
+class MulAdd : public Layer
+{
 
 public:
     MulAdd(Network *net, dnnType mul, dnnType add);
@@ -206,12 +311,11 @@ protected:
     dnnType *add_vector;
 };
 
-
-
 /**
     Avaible pooling functions (padding on tkDNN is not supported)
 */
-typedef enum {
+typedef enum
+{
     POOLING_MAX = 0,
     POOLING_AVERAGE = 1,                // count for average includes padded values
     POOLING_AVERAGE_EXCLUDE_PADDING = 2 // count for average does not include padded values
@@ -221,7 +325,8 @@ typedef enum {
     Pooling layer
     currenty supported only 2d pooing (also on 3d input)
 */
-class Pooling : public Layer {
+class Pooling : public Layer
+{
 
 public:
     int winH, winW;
@@ -236,7 +341,6 @@ public:
     virtual dnnType *infer(dataDim_t &dim, dnnType *srcData);
 
 protected:
-
     cudnnPoolingDescriptor_t poolingDesc;
     tkdnnPoolingMode_t pool_mode;
     dnnType *tmpInputData, *tmpOutputData;
@@ -246,7 +350,8 @@ protected:
 /**
     Softmax layer
 */
-class Softmax : public Layer {
+class Softmax : public Layer
+{
 
 public:
     Softmax(Network *net);
@@ -260,7 +365,8 @@ public:
     Route layer
     Merge a list of layers
 */
-class Route : public Layer {
+class Route : public Layer
+{
 
 public:
     Route(Network *net, Layer **layers, int layers_n);
@@ -274,12 +380,12 @@ public:
     int layers_n;   //number of layers
 };
 
-
 /**
     Reorg layer
     Mantain same dimension but change C*H*W distribution
 */
-class Reorg : public Layer {
+class Reorg : public Layer
+{
 
 public:
     Reorg(Network *net, int stride);
@@ -295,7 +401,8 @@ public:
     Shortcut layer
     sum with stride another layer
 */
-class Shortcut : public Layer {
+class Shortcut : public Layer
+{
 
 public:
     Shortcut(Network *net, Layer *backLayer);
@@ -312,7 +419,8 @@ public:
     Upsample layer
     Mantain same dimension but change C*H*W distribution
 */
-class Upsample : public Layer {
+class Upsample : public Layer
+{
 
 public:
     Upsample(Network *net, int stride);
@@ -325,12 +433,14 @@ public:
     bool reverse;
 };
 
-struct box {
+struct box
+{
     int cl;
     float x, y, w, h;
     float prob;
 };
-struct sortable_bbox {
+struct sortable_bbox
+{
     int index;
     int cl;
     float **probs;
@@ -339,14 +449,17 @@ struct sortable_bbox {
 /**
     Yolo3 layer
 */
-class Yolo : public Layer {
+class Yolo : public Layer
+{
 
 public:
-    struct box {
+    struct box
+    {
         float x, y, w, h;
     };
 
-    struct detection{
+    struct detection
+    {
         Yolo::box bbox;
         int classes;
         float *prob;
@@ -377,7 +490,8 @@ public:
 /**
     Region layer
 */
-class Region : public Layer {
+class Region : public Layer
+{
 
 public:
     Region(Network *net, int classes, int coords, int num);
@@ -389,7 +503,8 @@ public:
     virtual dnnType *infer(dataDim_t &dim, dnnType *srcData);
 };
 
-class RegionInterpret {
+class RegionInterpret
+{
 
 public:
     RegionInterpret(dataDim_t input_dim, dataDim_t output_dim, 
@@ -401,7 +516,6 @@ public:
     int classes, coords, num;
     float thresh;
 
-    
     box *boxes;
     float **probs;
     sortable_bbox *s;
@@ -419,5 +533,6 @@ public:
     static float box_iou(box a, box b);
 };
 
-}}
+} // namespace dnn
+} // namespace tk
 #endif //LAYER_H

include/tkDNN/Network.h

@@ -3,7 +3,10 @@
 
 #include "utils.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
 /**
     Data rapresentation beetween layers
@@ -13,20 +16,22 @@ namespace tk { namespace dnn {
     w = width  (rows)
     l = lenght (3rd dimension)
 */
-struct dataDim_t {
+struct dataDim_t
+{
 
     int n, c, h, w, l;
 
     dataDim_t() : n(1), c(1), h(1), w(1), l(1){};
 
-    dataDim_t(int _n, int _c, int _h, int _w, int _l = 1) :
+    dataDim_t(int _n, int _c, int _h, int _w, int _l = 1) : n(_n), c(_c), h(_h), w(_w), l(_l){};
-        n(_n), c(_c), h(_h), w(_w), l(_l) {};
 
-    void print() {
+    void print()
+    {
         std::cout << "Data dim: " << n << " " << c << " " << h << " " << w << " " << l << "\n";
     }
 
-    int tot() {
+    int tot()
+    {
         return n * c * h * w * l;
     }
 };
@@ -34,7 +39,8 @@ struct dataDim_t {
 class Layer;
 const int MAX_LAYERS = 256;
 
-class Network {
+class Network
+{
 
 public:
     Network(dataDim_t input_dim);
@@ -62,5 +68,6 @@ public:
     bool fp16, dla;
 };
 
-}}
+} // namespace dnn
+} // namespace tk
 #endif //NETWORK_H

include/tkDNN/NetworkRT.h

@@ -7,15 +7,20 @@
 #include "Layer.h"
 #include "NvInfer.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
-template<typename T> void writeBUF(char*& buffer, const T& val)
+template <typename T>
+void writeBUF(char *&buffer, const T &val)
 {
     *reinterpret_cast<T *>(buffer) = val;
     buffer += sizeof(T);
 }
 
-template<typename T> T readBUF(const char*& buffer)
+template <typename T>
+T readBUF(const char *&buffer)
 {
     T val = *reinterpret_cast<const T *>(buffer);
     buffer += sizeof(T);
@@ -41,9 +46,8 @@ public:
     virtual IPlugin *createPlugin(const char *layerName, const void *serialData, size_t serialLength);
 };
 
-
+class NetworkRT
-
+{
-class NetworkRT {
 
 public:
     nvinfer1::DataType dtRT;
@@ -90,5 +94,6 @@ public:
     bool deserialize(const char *filename);
 };
 
-}}
+} // namespace dnn
+} // namespace tk
 #endif //NETWORKRT_H

include/tkDNN/Yolo3Detection.h

@@ -1,3 +1,6 @@
+#ifndef YOLO3DDETECTION_H
+#define YOLO3DDETECTION_H
+
 #include <iostream>
 #include <signal.h>
 #include <stdlib.h> /* srand, rand */
@@ -11,13 +14,17 @@
 
 #include "tkdnn.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
 /**
  * 
  * @author Francesco Gatti
  */
-class Yolo3Detection {
+class Yolo3Detection
+{
 
 private:
     tk::dnn::NetworkRT *netRT = nullptr;
@@ -47,14 +54,15 @@ class Yolo3Detection {
     virtual ~Yolo3Detection() {}
 
     /**
-         * Method used for inizialize the class
+         * Method used to inizialize the class
          * 
          * @return Success of the initialization
          */
     bool init(std::string tensor_path);
-
+    void addBorders(cv::Mat &imageORIG, cv::Mat &imageWBorders, int &top, int &left);
     void update(cv::Mat &frame);
 
+
     tk::dnn::Yolo* getYoloLayer(int n=0) {
         if(n<3)
             return yolo[n];
@@ -64,4 +72,7 @@ class Yolo3Detection {
 
 };
 
-}}
+} // namespace dnn
+} // namespace tk
+
+#endif /*YOLO3DDETECTION_H*/

include/tkDNN/utils.h

@@ -32,13 +32,17 @@
 #define COL_CYANB "\033[1;36m"
 
 // Simple Timer
-#define TIMER_START timespec start, end;                               \
+#define TIMER_START    \
+  timespec start, end; \
   clock_gettime(CLOCK_MONOTONIC, &start);
 
-#define TIMER_STOP_C(col)  clock_gettime(CLOCK_MONOTONIC, &end);       \
+#define TIMER_STOP_C(col)                                         \
+  clock_gettime(CLOCK_MONOTONIC, &end);                           \
   double t_ns = ((double)(end.tv_sec - start.tv_sec) * 1.0e9 +    \
-                  (double)(end.tv_nsec - start.tv_nsec))/1.0e6;        \
+                 (double)(end.tv_nsec - start.tv_nsec)) /         \
-    std::cout<<col<<"Time:"<<std::setw(16)<<t_ns<<" ms\n"<<COL_END; 
+                1.0e6;                                            \
+  std::cout << col << "Time:" << std::setw(16) << t_ns << " ms\n" \
+            << COL_END;
 
 #define TIMER_STOP TIMER_STOP_C(COL_CYANB)
 
@@ -47,42 +51,52 @@
  * ******************************************************/
 #define EXIT_WAIVED 0
 
-#define FatalError(s) {                                                \
+#define FatalError(s)                                 \
+  {                                                   \
     std::stringstream _where, _message;               \
     _where << __FILE__ << ':' << __LINE__;            \
-    _message << std::string(s) + "\n" << __FILE__ << ':' << __LINE__;\
+    _message << std::string(s) + "\n"                 \
+             << __FILE__ << ':' << __LINE__;          \
     std::cerr << _message.str() << "\nAborting...\n"; \
     cudaDeviceReset();                                \
     exit(EXIT_FAILURE);                               \
   }
 
-#define checkCUDNN(status) {                                           \
+#define checkCUDNN(status)                                        \
+  {                                                               \
     std::stringstream _error;                                     \
-    if (status != CUDNN_STATUS_SUCCESS) {                              \
+    if (status != CUDNN_STATUS_SUCCESS)                           \
+    {                                                             \
       _error << "CUDNN failure: " << cudnnGetErrorString(status); \
       FatalError(_error.str());                                   \
     }                                                             \
   }
 
-#define checkCuda(status) {                                            \
+#define checkCuda(status)                                       \
+  {                                                             \
     std::stringstream _error;                                   \
-    if (status != 0) {                                                 \
+    if (status != 0)                                            \
+    {                                                           \
       _error << "Cuda failure: " << cudaGetErrorString(status); \
       FatalError(_error.str());                                 \
     }                                                           \
   }
 
-#define checkERROR(status) {                                           \
+#define checkERROR(status)                     \
+  {                                            \
     std::stringstream _error;                  \
-    if (status != 0) {                                                 \
+    if (status != 0)                           \
+    {                                          \
       _error << "Generic failure: " << status; \
       FatalError(_error.str());                \
     }                                          \
   }
 
-#define checkNULL(ptr) {                                               \
+#define checkNULL(ptr)          \
+  {                             \
     std::stringstream _error;   \
-    if (ptr == nullptr) {                                              \
+    if (ptr == nullptr)         \
+    {                           \
       _error << "Null pointer"; \
       FatalError(_error.str()); \
     }                           \
@@ -91,7 +105,7 @@
 void printCenteredTitle(const char *title, char fill, int dim);
 bool fileExist(const char *fname);
 void readBinaryFile(std::string fname, int size, dnnType** data_h, dnnType** data_d, int seek = 0);
-int checkResult(int size, dnnType *data_d, dnnType *correct_d, bool device = true);
+int checkResult(int size, dnnType *data_d, dnnType *correct_d, bool device = true, int limit = 10);
 void printDeviceVector(int size, dnnType* vec_d, bool device = true);
 void resize(int size, dnnType **data);
 

include/visualization.h

@@ -0,0 +1,42 @@
+#ifndef VIZUALIZATION_H
+#define VIZUALIZATION_H
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+//saliency
+#include <opencv2/core/utility.hpp>
+//#include <opencv2/saliency.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <chrono>
+#include <iostream>
+#include <cstring>
+
+#include "tracker.h"
+#include "cameraUtils.h"
+#include "calibration.h"
+#include "boxDetection.h"
+
+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;
+};
+
+extern Show_t updates;
+extern bool gRun;
+extern std::string obj_class[10];
+
+/* Thread function to show the updated images 
+**/
+void *show_updates(void *x_void_ptr);
+void *originalFrame(void *x_void_ptr);
+void *detectionFrame(void *x_void_ptr);
+void *topviewFrame(void *x_void_ptr);
+void *disparityFrame(void *x_void_ptr);
+
+#endif /*VIZUALIZATION_H*/

src/Activation.cpp

@@ -3,15 +3,19 @@
 #include "Layer.h"
 #include "kernels.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
-Activation::Activation(Network *net, int act_mode) : 
+Activation::Activation(Network *net, int act_mode) : Layer(net)
-    Layer(net) {
+{
 
     this->act_mode = act_mode;
     checkCuda(cudaMalloc(&dstData, input_dim.tot() * sizeof(dnnType)));
 
-    if(int(act_mode) < 100) {
+    if (int(act_mode) < 100)
+    {
 
         checkCUDNN(cudnnSetTensor4dDescriptor(srcTensorDesc,
                                               net->tensorFormat,
@@ -26,7 +30,6 @@ Activation::Activation(Network *net, int act_mode) :
                                               input_dim.c,
                                               input_dim.h, input_dim.w));
 
-
         checkCUDNN(cudnnCreateActivationDescriptor(&activDesc));
         checkCUDNN(cudnnSetActivationDescriptor(activDesc,
                                                 (cudnnActivationMode_t)act_mode,
@@ -35,7 +38,8 @@ Activation::Activation(Network *net, int act_mode) :
     }
 }
 
-Activation::~Activation() {
+Activation::~Activation()
+{
 
     checkCuda(cudaFree(dstData));
 
@@ -43,12 +47,15 @@ Activation::~Activation() {
         checkCUDNN(cudnnDestroyActivationDescriptor(activDesc));
 }
 
-dnnType* Activation::infer(dataDim_t &dim, dnnType* srcData) {
+dnnType *Activation::infer(dataDim_t &dim, dnnType *srcData)
+{
 
-    if(act_mode == ACTIVATION_LEAKY) {
+    if (act_mode == ACTIVATION_LEAKY)
+    {
         activationLEAKYForward(srcData, dstData, dim.tot());
-
+    }
-    } else {
+    else
+    {
         dnnType alpha = dnnType(1);
         dnnType beta = dnnType(0);
         checkCUDNN(cudnnActivationForward(net->cudnnHandle,
@@ -63,4 +70,5 @@ dnnType* Activation::infer(dataDim_t &dim, dnnType* srcData) {
     return dstData;
 }
 
-}}
+} // namespace dnn
+} // namespace tk

src/Conv2d.cpp

@@ -2,7 +2,10 @@
 
 #include "Layer.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
 Conv2d::Conv2d( Network *net, int out_ch, int kernelH, int kernelW, 
                 int strideH, int strideW, int paddingH, int paddingW,
@@ -59,16 +62,15 @@ Conv2d::Conv2d( Network *net, int out_ch, int kernelH, int kernelW,
                                                        srcTensorDesc, filterDesc, convDesc, dstTensorDesc,
                                                        algo, &ws_sizeInBytes));
 
-    if (ws_sizeInBytes!=0) {
+    if (ws_sizeInBytes != 0)
+    {
         checkCuda(cudaMalloc(&workSpace, ws_sizeInBytes));
     }
 
-
     checkCUDNN(cudnnSetTensor4dDescriptor(biasTensorDesc,
                                           net->tensorFormat, net->dataType,
                                           1, out_ch, 1, 1));
 
-
     output_dim.n = n;
     output_dim.c = c;
     output_dim.h = h;
@@ -79,7 +81,8 @@ Conv2d::Conv2d( Network *net, int out_ch, int kernelH, int kernelW,
     checkCuda(cudaMalloc(&dstData, output_dim.tot() * sizeof(dnnType)));
 }
 
-Conv2d::~Conv2d() {
+Conv2d::~Conv2d()
+{
 
     checkCUDNN(cudnnDestroyFilterDescriptor(filterDesc));
     checkCUDNN(cudnnDestroyConvolutionDescriptor(convDesc));
@@ -91,8 +94,8 @@ Conv2d::~Conv2d() {
     checkCuda(cudaFree(dstData));
 }
 
-dnnType* Conv2d::infer(dataDim_t &dim, dnnType* srcData) {
+dnnType *Conv2d::infer(dataDim_t &dim, dnnType *srcData)
-
+{
 
     // convolution
     dnnType alpha = dnnType(1);
@@ -102,14 +105,17 @@ dnnType* Conv2d::infer(dataDim_t &dim, dnnType* srcData) {
                                        data_d, convDesc, algo, workSpace, ws_sizeInBytes,
                                        &beta, dstTensorDesc, dstData));
 
-    if(!batchnorm) {
+    if (!batchnorm)
+    {
         // bias
         alpha = dnnType(1);
         beta = dnnType(1);
         checkCUDNN(cudnnAddTensor(net->cudnnHandle,
                                   &alpha, biasTensorDesc, bias_d,
                                   &beta, dstTensorDesc, dstData));
-    } else {
+    }
+    else
+    {
         float one = 1;
         float zero = 0;
         cudnnBatchNormalizationForwardInference(net->cudnnHandle,
@@ -125,4 +131,5 @@ dnnType* Conv2d::infer(dataDim_t &dim, dnnType* srcData) {
     return dstData;
 }
 
-}}
+} // namespace dnn
+} // namespace tk

src/Dense.cpp

@@ -2,7 +2,10 @@
 
 #include "Layer.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
 Dense::Dense(Network *net, int out_ch, std::string fname_weights) : 
     LayerWgs(net, net->getOutputDim().tot(), out_ch, 1, 1, 1, fname_weights) {
@@ -17,12 +20,14 @@ Dense::Dense(Network *net, int out_ch, std::string fname_weights) :
     checkCuda(cudaMalloc(&dstData, output_dim.tot() * sizeof(dnnType)));
 }
 
-Dense::~Dense() {
+Dense::~Dense()
+{
 
     checkCuda(cudaFree(dstData));
 }
 
-dnnType* Dense::infer(dataDim_t &dim, dnnType* srcData) {
+dnnType *Dense::infer(dataDim_t &dim, dnnType *srcData)
+{
 
     if (dim.n != 1)
         FatalError("Not Implemented");
@@ -55,4 +60,5 @@ dnnType* Dense::infer(dataDim_t &dim, dnnType* srcData) {
     return dstData;
 }
 
-}}
+} // namespace dnn
+} // namespace tk

src/Flatten.cpp

@@ -3,9 +3,13 @@
 #include "Layer.h"
 #include "kernels.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
-Flatten::Flatten(Network *net) : Layer(net) {
+Flatten::Flatten(Network *net) : Layer(net)
+{
 
     checkCuda(cudaMalloc(&dstData, input_dim.tot() * sizeof(dnnType)));
 
@@ -14,15 +18,16 @@ Flatten::Flatten(Network *net) : Layer(net) {
     output_dim.h = 1;
     output_dim.w = 1;
     output_dim.l = 1;
-
 }
 
-Flatten::~Flatten() {
+Flatten::~Flatten()
+{
 
     checkCuda(cudaFree(dstData));
 }
 
-dnnType* Flatten::infer(dataDim_t &dim, dnnType* srcData) {
+dnnType *Flatten::infer(dataDim_t &dim, dnnType *srcData)
+{
 
     //transpose per channel
     matrixTranspose(net->cublasHandle, srcData, dstData, dim.c, dim.h * dim.w * dim.l);
@@ -33,4 +38,5 @@ dnnType* Flatten::infer(dataDim_t &dim, dnnType* srcData) {
     return dstData;
 }
 
-}}
+} // namespace dnn
+} // namespace tk

src/LSTM.cpp

@@ -0,0 +1,327 @@
+#include <iostream>
+
+#include "Layer.h"
+
+namespace tk { namespace dnn {
+
+LSTM::LSTM( Network *net, int hiddensize, bool returnSeq, std::string fname_weights) :
+    Layer(net) {
+
+    this->returnSeq = returnSeq;
+    int batchSize = input_dim.n;
+    int inputSize = input_dim.c;
+    seqLen        = input_dim.w;
+    stateSize     = hiddensize;
+
+    // init Tensor Descriptors
+    std::vector<cudnnTensorDescriptor_t> x_vec(seqLen);
+    std::vector<cudnnTensorDescriptor_t> y_vec(seqLen);
+
+    int dimA[3];
+    int strideA[3];
+    for (int i = 0; i < seqLen; i++) {
+        checkCUDNN(cudnnCreateTensorDescriptor(&x_vec[i]));
+        checkCUDNN(cudnnCreateTensorDescriptor(&y_vec[i]));
+
+        dimA[0] = batchSize;
+        dimA[1] = inputSize;
+        dimA[2] = 1;
+        dimA[0] = batchSize;
+        dimA[1] = inputSize;
+        strideA[0] = dimA[2] * dimA[1];
+        strideA[1] = dimA[2];
+        strideA[2] = 1;
+        checkCUDNN(cudnnSetTensorNdDescriptor(x_vec[i],
+            net->dataType, 3, dimA, strideA));
+
+        dimA[0] = batchSize;
+        dimA[1] = stateSize;
+        dimA[2] = 1;
+        strideA[0] = dimA[2] * dimA[1];
+        strideA[1] = dimA[2];
+        strideA[2] = 1;
+        checkCUDNN(cudnnSetTensorNdDescriptor(y_vec[i],
+            net->dataType, 3, dimA, strideA));
+    }
+    // apply tensordesc
+    x_desc_vec_ = x_vec;
+    y_desc_vec_ = y_vec;
+
+
+    // set the state tensors
+    dimA[0] = numLayers;
+    dimA[1] = batchSize;
+    dimA[2] = stateSize;
+    strideA[0] = dimA[2] * dimA[1];
+    strideA[1] = dimA[2];
+    strideA[2] = 1;
+    checkCUDNN(cudnnCreateTensorDescriptor(&hx_desc_));
+    checkCUDNN(cudnnCreateTensorDescriptor(&cx_desc_));
+    checkCUDNN(cudnnCreateTensorDescriptor(&hy_desc_));
+    checkCUDNN(cudnnCreateTensorDescriptor(&cy_desc_));
+    checkCUDNN(cudnnSetTensorNdDescriptor(hx_desc_, net->dataType, 3, dimA, strideA));
+    checkCUDNN(cudnnSetTensorNdDescriptor(cx_desc_, net->dataType, 3, dimA, strideA));
+    checkCUDNN(cudnnSetTensorNdDescriptor(hy_desc_, net->dataType, 3, dimA, strideA));
+    checkCUDNN(cudnnSetTensorNdDescriptor(cy_desc_, net->dataType, 3, dimA, strideA));
+    // allocate     dnnType *hx_ptr, *cx_ptr, *hy_ptr, *cy_ptr;
+    stateDataDim = dimA[0]*dimA[1]*dimA[2];
+    checkCuda( cudaMalloc(&hx_ptr, stateDataDim*sizeof(dnnType)) );
+    checkCuda( cudaMalloc(&cx_ptr, stateDataDim*sizeof(dnnType)) );
+    checkCuda( cudaMalloc(&hy_ptr, stateDataDim*sizeof(dnnType)) );
+    checkCuda( cudaMalloc(&cy_ptr, stateDataDim*sizeof(dnnType)) );
+    
+
+
+    // Create Dropout descriptors // TODO: ??? IS IT NECESSARY ???
+    float dropoutprob = 0.1f; // random val ????
+    checkCUDNN(cudnnCreateDropoutDescriptor(&dropoutDesc));
+    checkCUDNN(cudnnDropoutGetStatesSize(net->cudnnHandle, &dropout_byte_));
+    dropout_size_ = dropout_byte_ / sizeof(dnnType);
+    checkCuda( cudaMalloc(&dropout_states_, dropout_byte_) );
+    uint64_t seed_ = 17 + rand() % 4096;  // NOLINT(runtime/threadsafe_fn)
+    checkCUDNN(cudnnSetDropoutDescriptor(dropoutDesc,
+        net->cudnnHandle, dropoutprob, dropout_states_, dropout_byte_, seed_));
+
+
+    // RNN descriptors
+    checkCUDNN(cudnnCreateRNNDescriptor(&rnnDesc));
+
+    checkCUDNN(cudnnSetRNNDescriptor(net->cudnnHandle, 
+        rnnDesc, stateSize, numLayers, dropoutDesc,
+        cudnnRNNInputMode_t::CUDNN_LINEAR_INPUT,
+        //(bidirectional ? cudnnDirectionMode_t::CUDNN_BIDIRECTIONAL : cudnnDirectionMode_t::CUDNN_UNIDIRECTIONAL),
+        cudnnDirectionMode_t::CUDNN_UNIDIRECTIONAL,
+        cudnnRNNMode_t::CUDNN_LSTM,
+        cudnnRNNAlgo_t::CUDNN_RNN_ALGO_STANDARD,
+        net->dataType));
+
+
+    // Get temp space sizes
+    checkCUDNN(cudnnGetRNNWorkspaceSize(net->cudnnHandle,
+        rnnDesc, seqLen, x_desc_vec_.data(), &workspace_byte_));
+    workspace_size_ = workspace_byte_ / sizeof(dnnType);
+    checkCuda( cudaMalloc(&work_space_, workspace_byte_) );    
+    
+
+    // Check that number of params are correct
+    size_t cudnn_param_size;
+    checkCUDNN(cudnnGetRNNParamsSize(net->cudnnHandle,
+        rnnDesc,x_desc_vec_[0], &cudnn_param_size, net->dataType));
+    int cudnn_params = cudnn_param_size/sizeof(dnnType);
+    //std::cout<<"LSTM params size: "<<cudnn_params << ", bytes: "<<cudnn_param_size<<"\n";
+
+    // Set param descriptors
+    checkCUDNN(cudnnCreateFilterDescriptor(&w_desc_));
+    int dim_w[3] = {1, 1, 1};
+    dim_w[0] = cudnn_params;
+    checkCUDNN(cudnnSetFilterNdDescriptor(w_desc_,
+        net->dataType, net->tensorFormat, 3, dim_w));
+
+    // load params
+    std::cout<<"Reading weights: PARAMS="<<cudnn_params*2<<"\n";
+    readBinaryFile(fname_weights, cudnn_params*2, &w_h, &w_ptr);
+    // set forward and backward params
+    wf_ptr = w_ptr;
+    wb_ptr = w_ptr + cudnn_params;
+    //std::cout<<"wf: "<<wf_ptr<<" wb "<<wb_ptr<<"\n"; 
+
+    // set output dim 
+    output_dim = input_dim;
+    output_dim.c = stateSize*(bidirectional ? 2 : 1);
+
+    // if retunseq is disabled only the last timestep is returned
+    if(!returnSeq) {
+        output_dim.h = 1;
+        output_dim.w = 1;
+    }
+
+    //allocate data for infer result
+    checkCuda( cudaMalloc(&dstData, output_dim.tot()*sizeof(dnnType)) );
+
+    // used during inference
+    one_output_dim = input_dim;
+    one_output_dim.c = stateSize;
+    checkCuda( cudaMalloc(&srcF, input_dim.tot()*sizeof(dnnType)) );
+    checkCuda( cudaMalloc(&srcB, input_dim.tot()*sizeof(dnnType)) );
+    checkCuda( cudaMalloc(&dstF, one_output_dim.tot()*sizeof(dnnType)) );
+    checkCuda( cudaMalloc(&dstB_NR, one_output_dim.tot()*sizeof(dnnType)) );
+    checkCuda( cudaMalloc(&dstB, one_output_dim.tot()*sizeof(dnnType)) );
+
+
+    /*
+    // Query weight layout
+    cudnnFilterDescriptor_t m_desc;
+    checkCUDNN(cudnnCreateFilterDescriptor(&m_desc));
+    dnnType *p;
+    int n = 8; // lstm layers
+    
+    printCenteredTitle("WEIGHTS", '=', 20);
+    for (int i = 0; i < numLayers; ++i) {
+        for (int j = 0; j < n; ++j) {
+            
+            checkCUDNN(cudnnGetRNNLinLayerMatrixParams(net->cudnnHandle, rnnDesc,
+                i, x_desc_vec_[0], w_desc_, 0, j, m_desc, (void**)&p));
+            
+            std::cout << "ptr: " << ((int64_t)(p - NULL))/sizeof(dnnType)<<"\n";
+            
+            cudnnDataType_t t;
+            cudnnTensorFormat_t f;
+            int ndim = 5;
+            int dims[5] = {0, 0, 0, 0, 0};
+            checkCUDNN(cudnnGetFilterNdDescriptor(m_desc, ndim, &t, &f, &ndim, &dims[0]));
+            std::cout << "(layer, linlayer): " <<  i << " " << j << "\n";
+
+            int tot = 1;
+            for (int i = 0; i < ndim; ++i) {
+                std::cout << dims[i] << " ";
+                tot *= dims[i];
+            }
+            std::cout<<"\t-> "<<tot<<"\n\n";
+        }
+    }
+
+    printCenteredTitle("BIAS", '=', 20);
+    for (int i = 0; i < numLayers; ++i) {
+        for (int j = 0; j < n; ++j) {
+            checkCUDNN(cudnnGetRNNLinLayerBiasParams(net->cudnnHandle, rnnDesc, 
+                i, x_desc_vec_[0], w_desc_, 0, j, m_desc, (void**)&p));
+
+            std::cout << "ptr: " << ((int64_t)(p - NULL))/sizeof(dnnType)<<"\n";
+
+            cudnnDataType_t t;
+            cudnnTensorFormat_t f;
+            int ndim = 5;
+            int dims[5] = {0, 0, 0, 0, 0};
+            checkCUDNN(cudnnGetFilterNdDescriptor(m_desc, ndim, &t, &f, &ndim, &dims[0]));
+            std::cout << "(layer, linlayer): " <<  i << " " << j << "\n";
+
+            int tot = 1;
+            for (int i = 0; i < ndim; ++i) {
+                std::cout << dims[i] << " ";
+                tot *= dims[i];
+            }
+            std::cout<<"\t-> "<<tot<<"\n\n";
+        }
+    }
+    
+    checkCUDNN(cudnnDestroyFilterDescriptor(m_desc));
+    */
+}
+
+LSTM::~LSTM() {
+    checkCuda(cudaFree(hx_ptr));
+    checkCuda(cudaFree(cx_ptr));
+    checkCuda(cudaFree(hy_ptr));
+    checkCuda(cudaFree(cy_ptr));
+    checkCuda(cudaFree(w_ptr ));
+
+    checkCuda(cudaFree(work_space_    ));
+    checkCuda(cudaFree(dropout_states_));
+
+    checkCuda(cudaFree(srcF));
+    checkCuda(cudaFree(srcB));
+    checkCuda(cudaFree(dstF));
+    checkCuda(cudaFree(dstB_NR));
+    checkCuda(cudaFree(dstB));
+    checkCuda(cudaFree(dstData));
+}
+
+dnnType* LSTM::infer(dataDim_t &dim, dnnType* srcData) {
+
+    // transpose input 
+    matrixTranspose(net->cublasHandle, srcData, srcF, dim.c, dim.h*dim.w*dim.l);
+    
+    // build srcB as reversed srcF
+    for(int i=0; i<input_dim.w; i++) {
+        int off_0 = i*(input_dim.c);
+        int off_1 = (i+1)*(input_dim.c);
+        checkCuda( cudaMemcpy(srcB + dim.tot() - off_1, srcF + off_0, 
+            input_dim.c*sizeof(dnnType), cudaMemcpyDeviceToDevice));
+    }
+
+    // forward
+    { 
+        // reset states
+        checkCuda( cudaMemset(hx_ptr, 0, stateDataDim*sizeof(float)) );	
+        checkCuda( cudaMemset(cx_ptr, 0, stateDataDim*sizeof(float)) );
+
+
+        checkCUDNN(cudnnRNNForwardInference(net->cudnnHandle,
+            rnnDesc,
+            seqLen,                     // number of time steps (nT)
+            x_desc_vec_.data(),         // input array of desc (nT*nC_in)
+            srcF,                    // input pointer
+            hx_desc_,                   // initial hidden state desc     
+            hx_ptr,                     // initial hidden state pointer 
+            cx_desc_,                   // initial cell state desc      
+            cx_ptr,                     // initial cell state pointer   
+            w_desc_,                    // weights desc
+            wf_ptr,                     // weights pointer
+            y_desc_vec_.data(),         // output desc     (nT*nC_out)
+            dstF,                       // output pointer
+            hy_desc_,                   // final hidden state desc        
+            hy_ptr,                     // final hidden state pointer 
+            cy_desc_,                   // final cell state desc          
+            cy_ptr,                     // final cell state pointer   
+            work_space_,                // workspace pointer
+            workspace_byte_));          // workspace size    
+    }
+
+    // backward
+    { 
+        // reset states
+        checkCuda( cudaMemset(hx_ptr, 0, stateDataDim*sizeof(float)) );	
+        checkCuda( cudaMemset(cx_ptr, 0, stateDataDim*sizeof(float)) );
+
+        checkCUDNN(cudnnRNNForwardInference(net->cudnnHandle,
+            rnnDesc,
+            seqLen,                     // number of time steps (nT)
+            x_desc_vec_.data(),         // input array of desc (nT*nC_in)
+            srcB,                    // input pointer
+            hx_desc_,                   // initial hidden state desc     
+            hx_ptr,                     // initial hidden state pointer 
+            cx_desc_,                   // initial cell state desc      
+            cx_ptr,                     // initial cell state pointer   
+            w_desc_,                    // weights desc
+            wb_ptr,                     // weights pointer
+            y_desc_vec_.data(),         // output desc     (nT*nC_out)
+            dstB_NR,                       // output pointer
+            hy_desc_,                   // final hidden state desc        
+            hy_ptr,                     // final hidden state pointer 
+            cy_desc_,                   // final cell state desc          
+            cy_ptr,                     // final cell state pointer   
+            work_space_,                // workspace pointer
+            workspace_byte_));          // workspace size    
+    }
+
+
+    // reverse order of dstB
+    for(int i=0; i<one_output_dim.w; i++) {
+        int off_0 = i*(one_output_dim.c);
+        int off_1 = (i+1)*(one_output_dim.c);
+        checkCuda( cudaMemcpy(dstB + one_output_dim.tot() - off_1, dstB_NR + off_0, 
+            one_output_dim.c*sizeof(dnnType), cudaMemcpyDeviceToDevice));
+    }    
+
+    // if retunseq is disabled only the last timestep is returned
+    if(returnSeq) {
+        // forward transpose
+        matrixTranspose(net->cublasHandle, dstF, dstData, 
+                         one_output_dim.h* one_output_dim.w*one_output_dim.l, one_output_dim.c);
+        // backward transpose
+        matrixTranspose(net->cublasHandle, dstB, dstData + one_output_dim.tot(), 
+                         one_output_dim.h* one_output_dim.w*one_output_dim.l, one_output_dim.c);    
+    } else {
+        // copy last of forward 
+        checkCuda( cudaMemcpy(dstData, dstF + one_output_dim.tot() - one_output_dim.c, 
+            one_output_dim.c*sizeof(dnnType), cudaMemcpyDeviceToDevice));
+        // copy first of backward
+        checkCuda( cudaMemcpy(dstData + one_output_dim.c, dstB,  
+            one_output_dim.c*sizeof(dnnType), cudaMemcpyDeviceToDevice));
+    }
+
+    dim = output_dim;
+    return dstData;
+}
+
+}}

src/Layer.cpp

@@ -2,13 +2,18 @@
 
 #include "Layer.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
-Layer::Layer(Network *net) {
+Layer::Layer(Network *net)
+{
 
     this->net = net;
 
-    if(net != nullptr) {
+    if (net != nullptr)
+    {
         this->input_dim = net->getOutputDim();
         this->output_dim = input_dim;
 
@@ -20,10 +25,12 @@ Layer::Layer(Network *net) {
     }
 }
 
-Layer::~Layer() {
+Layer::~Layer()
+{
 
     checkCUDNN(cudnnDestroyTensorDescriptor(srcTensorDesc));
     checkCUDNN(cudnnDestroyTensorDescriptor(dstTensorDesc));
 }
 
-}}
+} // namespace dnn
+} // namespace tk

src/LayerWgs.cpp

@@ -4,7 +4,10 @@
 #include "Layer.h"
 #include "kernels.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
 LayerWgs::LayerWgs(Network *net, int inputs, int outputs, 
                    int kh, int kw, int kl, 
@@ -21,7 +24,8 @@ LayerWgs::LayerWgs(Network *net, int inputs, int outputs,
     readBinaryFile(weights_path.c_str(), outputs, &bias_h, &bias_d, seek);
 
     this->batchnorm = batchnorm;
-    if(batchnorm) {
+    if (batchnorm)
+    {
         seek += outputs;
         readBinaryFile(weights_path.c_str(), outputs, &scales_h, &scales_d, seek);
         seek += outputs;
@@ -32,7 +36,8 @@ LayerWgs::LayerWgs(Network *net, int inputs, int outputs,
         float eps = TKDNN_BN_MIN_EPSILON;
 
         power_h = new dnnType[outputs];
-        for(int i=0; i<outputs; i++) power_h[i] = 1.0f;
+        for (int i = 0; i < outputs; i++)
+            power_h[i] = 1.0f;
 
         for (int i = 0; i < outputs; i++)
             mean_h[i] = mean_h[i] / -sqrt(eps + variance_h[i]);
@@ -41,7 +46,6 @@ LayerWgs::LayerWgs(Network *net, int inputs, int outputs,
             variance_h[i] = 1.0f / sqrt(eps + variance_h[i]);
     }
 
-
     if (!net->fp16)
         return;
 
@@ -58,7 +62,8 @@ LayerWgs::LayerWgs(Network *net, int inputs, int outputs,
     float2half(bias_d, bias16_d, b_size);
     cudaMemcpy(bias16_h, bias16_d, b_size * sizeof(__half), cudaMemcpyDeviceToHost);
 
-    if(batchnorm) {
+    if (batchnorm)
+    {
 
         power16_h = new __half[b_size];
         mean16_h = new __half[b_size];
@@ -97,14 +102,16 @@ LayerWgs::LayerWgs(Network *net, int inputs, int outputs,
     }
 }
 
-LayerWgs::~LayerWgs() {
+LayerWgs::~LayerWgs()
+{
 
     delete[] data_h;
     delete[] bias_h;
     checkCuda(cudaFree(data_d));
     checkCuda(cudaFree(bias_d));
 
-    if(batchnorm) {
+    if (batchnorm)
+    {
         delete[] scales_h;
         delete[] mean_h;
         delete[] variance_h;
@@ -114,4 +121,5 @@ LayerWgs::~LayerWgs() {
     }
 }
 
-}}
+} // namespace dnn
+} // namespace tk

src/MulAdd.cpp

@@ -3,9 +3,13 @@
 #include "Layer.h"
 #include "kernels.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
-MulAdd::MulAdd(Network *net, dnnType mul, dnnType add) : Layer(net) {
+MulAdd::MulAdd(Network *net, dnnType mul, dnnType add) : Layer(net)
+{
 
     this->mul = mul;
     this->add = add;
@@ -21,17 +25,18 @@ MulAdd::MulAdd(Network *net, dnnType mul, dnnType add) : Layer(net) {
     checkCuda(cudaMemcpy(add_vector, add_vector_h, size * sizeof(dnnType), cudaMemcpyHostToDevice));
     delete[] add_vector_h;
 
-
     checkCuda(cudaMalloc(&dstData, input_dim.tot() * sizeof(dnnType)));
 }
 
-MulAdd::~MulAdd() {
+MulAdd::~MulAdd()
+{
 
     checkCuda(cudaFree(add_vector));
     checkCuda(cudaFree(dstData));
 }
 
-dnnType* MulAdd::infer(dataDim_t &dim, dnnType* srcData) {
+dnnType *MulAdd::infer(dataDim_t &dim, dnnType *srcData)
+{
 
     matrixMulAdd(net->cublasHandle, srcData, dstData, add_vector, input_dim.tot(), mul);
 
@@ -41,4 +46,5 @@ dnnType* MulAdd::infer(dataDim_t &dim, dnnType* srcData) {
     return dstData;
 }
 
-}}
+} // namespace dnn
+} // namespace tk

src/Network.cpp

@@ -5,9 +5,13 @@
 #include "Network.h"
 #include "Layer.h"
 
-namespace tk { namespace dnn {
+namespace tk
+{
+namespace dnn
+{
 
-Network::Network(dataDim_t input_dim) {
+Network::Network(dataDim_t input_dim)
+{
     this->input_dim = input_dim;
 
     float tk_ver = float(TKDNN_VERSION) / 1000;
@@ -21,10 +25,12 @@ Network::Network(dataDim_t input_dim) {
 
     fp16 = false;
     dla = false;
-    if(const char* env_p = std::getenv("TKDNN_MODE")) {
+    if (const char *env_p = std::getenv("TKDNN_MODE"))
+    {
         if (strcmp(env_p, "FP16") == 0)
             fp16 = true;
-	else if(strcmp(env_p, "DLA") == 0) {
+        else if (strcmp(env_p, "DLA") == 0)
+        {
             dla = true;
             fp16 = true;
         }
@@ -41,23 +47,27 @@ Network::Network(dataDim_t input_dim) {
 
 }
 
-Network::~Network() {
+Network::~Network()
+{
 
     checkCUDNN(cudnnDestroy(cudnnHandle));
     checkERROR(cublasDestroy(cublasHandle));
 }
 
-dnnType* Network::infer(dataDim_t &dim, dnnType* data) {
+dnnType *Network::infer(dataDim_t &dim, dnnType *data)
+{
 
     //do infer for every layer
-    for(int i=0; i<num_layers; i++) {
+    for (int i = 0; i < num_layers; i++)
+    {
         data = layers[i]->infer(dim, data);
     }
     checkCuda(cudaDeviceSynchronize());
     return data;
 }
 
-bool Network::addLayer(Layer *l) {
+bool Network::addLayer(Layer *l)
+{
     if (num_layers == MAX_LAYERS)
         return false;
 
@@ -65,7 +75,8 @@ bool Network::addLayer(Layer *l) {
     return true;
 }
 
-dataDim_t Network::getOutputDim() {
+dataDim_t Network::getOutputDim()
+{
 
     if (num_layers == 0)
         return input_dim;
@@ -73,39 +84,53 @@ dataDim_t Network::getOutputDim() {
         return layers[num_layers - 1]->output_dim;
 }
 
-void Network::print() {
+void Network::print()
+{
 
     printCenteredTitle(" NETWORK MODEL ", '=', 60);
-    std::cout.width(3); std::cout<<std::left<<"N.";
+    std::cout.width(3);
+    std::cout << std::left << "N.";
     std::cout << " ";
-    std::cout.width(17); std::cout<<std::left<<"Layer type";
+    std::cout.width(17);
-    std::cout.width(22); std::cout<<std::left<<"input (H*W,CH)";
+    std::cout << std::left << "Layer type";
-    std::cout.width(16); std::cout<<std::left<<"output (H*W,CH)";
+    std::cout.width(22);
+    std::cout << std::left << "input (H*W,CH)";
+    std::cout.width(16);
+    std::cout << std::left << "output (H*W,CH)";
     std::cout << "\n";
 
-    for(int i=0; i<num_layers; i++) {
+    for (int i = 0; i < num_layers; i++)
+    {
         dataDim_t in = layers[i]->input_dim;
         dataDim_t out = layers[i]->output_dim;
 
-        std::cout.width(3); std::cout<<std::right<<i;
+        std::cout.width(3);
+        std::cout << std::right << i;
         std::cout << " ";
-        std::cout.width(16); std::cout<<std::left<<layers[i]->getLayerName();
+        std::cout.width(16);
-        std::cout.width(4);  std::cout<<std::right<<in.h;
+        std::cout << std::left << layers[i]->getLayerName();
+        std::cout.width(4);
+        std::cout << std::right << in.h;
         std::cout << " x ";
-        std::cout.width(4);  std::cout<<std::right<<in.w;
+        std::cout.width(4);
+        std::cout << std::right << in.w;
         std::cout << ", ";
-        std::cout.width(4);  std::cout<<std::right<<in.c;
+        std::cout.width(4);
+        std::cout << std::right << in.c;
         std::cout << "  -> ";
-        std::cout.width(4);  std::cout<<std::right<<out.h;
+        std::cout.width(4);
+        std::cout << std::right << out.h;
         std::cout << " x ";
-        std::cout.width(4);  std::cout<<std::right<<out.w;
+        std::cout.width(4);
+        std::cout << std::right << out.w;
         std::cout << ", ";
-        std::cout.width(4);  std::cout<<std::right<<out.c;
+        std::cout.width(4);
+        std::cout << std::right << out.c;
         std::cout << "\n";
     }
     printCenteredTitle("", '=', 60);
     std::cout << "\n";
 }
 
-
+} // namespace dnn
-}}
+} // namespace tk

src/NetworkRT.cpp

@@ -275,18 +275,18 @@ ILayer* NetworkRT::convert_layer(ITensor *input, Activation *l) {
     if(l->act_mode == ACTIVATION_LEAKY) {
         //std::cout<<"New plugin LEAKY\n";
         
-        /*
+#if NV_TENSORRT_MAJOR < 6                
         // plugin version
         IPlugin *plugin = new ActivationLeakyRT();
         IPluginLayer *lRT = networkRT->addPlugin(&input, 1, *plugin);
         checkNULL(lRT);
         return lRT;
-        */
+#else 
-
         IActivationLayer *lRT = networkRT->addActivation(*input, ActivationType::kLEAKY_RELU);
         lRT->setAlpha(0.1);
         checkNULL(lRT);
         return lRT;
+#endif
 
     } else if(l->act_mode == CUDNN_ACTIVATION_RELU) {
         IActivationLayer *lRT = networkRT->addActivation(*input, ActivationType::kRELU);

src/Yolo3Detection.cpp

@@ -58,6 +58,60 @@ bool Yolo3Detection::init(std::string tensor_path) {
     return true;
 } 
 
+void Yolo3Detection::addBorders(cv::Mat &imageORIG, cv::Mat &imageWBorders, int &top, int &left)
+{
+    float net_ratio = float(netRT->input_dim.w)/float(netRT->input_dim.h);
+    float img_ratio = float(imageORIG.cols)/float(imageORIG.rows);
+    int bottom=0, right=0, diff= 0;
+    top=0, left=0;
+
+    //printf("%f %f\n", net_ratio, img_ratio);
+
+    if(net_ratio != img_ratio)
+    {
+        if(netRT->input_dim.w> netRT->input_dim.h)
+        {	
+            if(img_ratio > net_ratio)
+            {
+                diff = std::abs((imageORIG.cols - net_ratio*imageORIG.rows)/net_ratio);
+                top = diff/2;
+                bottom = diff/2 + diff%2;
+            }
+            else	
+            {
+                diff = std::abs(net_ratio*float(imageORIG.rows) - float(imageORIG.cols));
+                left = diff/2;
+                right = diff/2 + diff%2;
+            }	
+        }
+        else
+        {
+            if(img_ratio < net_ratio)
+            {
+                diff = std::abs((imageORIG.cols - net_ratio*imageORIG.rows)/net_ratio);
+                left = diff/2;
+                right = diff/2 + diff%2;
+            }
+            else	
+            {
+                diff = std::abs(net_ratio*float(imageORIG.rows) - float(imageORIG.cols));
+                top = diff/2;
+                bottom = diff/2 + diff%2;
+            }
+        }
+    
+    }
+
+    //printf("%d %d %d %d %d \n", diff, top, bottom, left, right);
+    imageWBorders = imageORIG;
+    copyMakeBorder( imageORIG, imageWBorders, top, bottom, left, right, cv::BORDER_CONSTANT, (0,0,0) );
+    //printf("%d %d\n", imageWBorders.cols, imageWBorders.rows);
+    //const char* window_name = "borders";
+    //cv::namedWindow( window_name, cv::WINDOW_AUTOSIZE );
+    //imshow( window_name, imageWBorders );
+    //cv::waitKey(0);
+}
+
 
 void Yolo3Detection::update(cv::Mat &imageORIG) {
 
@@ -65,13 +119,21 @@ void Yolo3Detection::update(cv::Mat &imageORIG) {
         std::cout<<"YOLO: NO IMAGE DATA\n";
         return;
     }     
-    float xRatio =  float(imageORIG.cols) / float(netRT->input_dim.w);
-    float yRatio =  float(imageORIG.rows) / float(netRT->input_dim.h);
     
-    resize(imageORIG, imageORIG, cv::Size(netRT->input_dim.w, netRT->input_dim.h));
+    int top, left;
+    cv::Mat imageWBorders;
+    addBorders(imageORIG, imageWBorders, top, left);
 
+    float xRatio =  float(imageWBorders.cols) / float(netRT->input_dim.w);
+    float yRatio =  float(imageWBorders.rows) / float(netRT->input_dim.h);
+
+    resize(imageWBorders, imageORIG, cv::Size(netRT->input_dim.w, netRT->input_dim.h));
     imageORIG.convertTo(imageF, CV_32FC3, 1/255.0); 
 
+    //const char* window_name = "resize";
+    //cv::namedWindow( window_name, cv::WINDOW_AUTOSIZE );
+    ///imshow( window_name, imageORIG );
+
     //split channels
     cv::split(imageF,bgr);//split source
 
@@ -131,10 +193,10 @@ void Yolo3Detection::update(cv::Mat &imageORIG) {
             //cv::rectangle(image, cv::Point(x0, y0), cv::Point(x1, y1), colors[obj_class], 2);
             
             // convert to image coords
-            x0 = xRatio*x0;
+            x0 = xRatio*x0 - left;
-            x1 = xRatio*x1;
+            x1 = xRatio*x1 - left;
-            y0 = yRatio*y0;
+            y0 = yRatio*y0 - top;
-            y1 = yRatio*y1;
+            y1 = yRatio*y1 - top;
               
             tk::dnn::box res;
             res.cl = obj_class;

src/class_src/boxDetection.cpp_wrong

@@ -0,0 +1,678 @@
+#include "boxDetection.h"
+#include <string.h>
+char buf_frame_crop_name[200];
+
+cv::Mat img_threshold(cv::Mat frame_crop)
+{
+    // Image Threshold Example
+    // https://docs.opencv.org/3.4/d7/d1c/tutorial_js_watershed.html
+    cv::Mat f = frame_crop.clone();
+    cv::Mat dst, gray;
+    // gray and threshold image
+    cv::cvtColor(f, gray, cv::COLOR_RGBA2GRAY, 0);
+    cv::threshold(gray, gray, 0, 255, cv::THRESH_BINARY_INV + cv::THRESH_OTSU);
+    return gray;
+}
+
+cv::Mat img_background(cv::Mat frame_crop)
+{
+    // Image Background Example
+    // https://docs.opencv.org/3.4/d7/d1c/tutorial_js_watershed.html
+    cv::Mat f = frame_crop.clone();
+    cv::Mat dst, gray, opening, coinsBg;
+    // gray and threshold image
+    cv::cvtColor(f, gray, cv::COLOR_RGBA2GRAY, 0);
+    cv::threshold(gray, gray, 0, 255, cv::THRESH_BINARY_INV + cv::THRESH_OTSU);
+    // get background
+    cv::Mat M = cv::Mat(3, 3, CV_8U, cv::Scalar(1, 1, 1, 1));
+    cv::erode(gray, opening, M);
+    cv::dilate(gray, opening, M);
+    cv::Point p = cv::Point(-1, -1);
+    cv::dilate(opening, coinsBg, M, p, 3);
+    return coinsBg;
+}
+
+cv::Mat img_dist_transform(cv::Mat frame_crop)
+{
+    // Distance Transform Example
+    // https://docs.opencv.org/3.4/d7/d1c/tutorial_js_watershed.html
+    cv::Mat f = frame_crop.clone();
+    cv::Mat dst, gray, opening, coinsBg, coinsFg, distTrans;
+    // gray and threshold image
+    cv::cvtColor(f, gray, cv::COLOR_RGBA2GRAY, 0);
+    cv::threshold(gray, gray, 0, 255, cv::THRESH_BINARY_INV + cv::THRESH_OTSU);
+    // cv::Mat::ones M(3,3,cv::CV_8U);
+    // get background
+    cv::Mat M = cv::Mat(3, 3, CV_8U, cv::Scalar(1, 1, 1, 1));
+    cv::erode(gray, opening, M);
+    cv::dilate(gray, opening, M);
+    cv::Point p = cv::Point(-1, -1);
+    cv::dilate(opening, coinsBg, M, p, 3);
+    // distance transorm
+    cv::distanceTransform(opening, distTrans, cv::DIST_L2, 5);
+    cv::normalize(distTrans, distTrans, 1, 0, cv::NORM_INF);
+    return distTrans;
+}
+
+// cv::Mat img_watershed(cv::Mat frame_crop)
+// {
+//     // Image Watershed Example
+//     // https://docs.opencv.org/3.4/d7/d1c/tutorial_js_watershed.html
+//     cv::Mat f = frame_crop.clone();
+//     cv::Mat dst, gray, opening, coinsBg, coinsFg, distTrans, unknown, markers;
+//     // gray and threshold image
+//     cv::cvtColor(f, gray, cv::COLOR_RGBA2GRAY, 0);
+//     cv::threshold(gray, gray, 0, 255, cv::THRESH_BINARY_INV + cv::THRESH_OTSU);
+//     // get background
+//     cv::Mat M = cv::Mat(3, 3, CV_8U, cv::Scalar(1,1,1,1));
+//     cv::erode(gray, opening, M);
+//     cv::dilate(gray, opening, M);
+//     cv::Point p = cv::Point(-1,-1);
+//     cv::dilate(opening, coinsBg, M, p, 3);
+//     // distance transorm
+//     cv::distanceTransform(opening, distTrans, cv::DIST_L2, 5);
+//     cv::normalize(distTrans, distTrans, 1, 0, cv::NORM_INF);
+
+//     // get foreground
+//     cv::threshold(distTrans, coinsFg, 0.7 * 1, 255, cv::THRESH_BINARY);
+//     coinsFg.convertTo(coinsFg, CV_8U, 1, 0);
+//     cv::subtract(coinsBg, coinsFg, unknown);
+//     // get connected components networks
+//     cv::connectedComponents(coinsFg, markers);
+//     // intptr_t n = NULL;
+//     for(int i = 0; i< markers.rows; i++)
+//     {
+//         for (int j = 0; j< markers.cols; j++)
+//         {
+//             M.at<uchar>(0, 0);
+//             markers.intPtr(i,j)[0] = markers.ucharPtr(i,j)[0] +1;
+//             if(unknown.ucharPtr(i,j)[0] == 255)
+//             {
+//                 markers.intPtr(i,j)[0] = 0;
+//             }
+//         }
+//     }
+//     cv::cvtColor(f, f, cv::COLOR_RGBA2RGB, 0);
+//     cv::watershed(f, markers);
+//     //draw barriers
+//     for(int i = 0; i< markers.rows; i++)
+//     {
+//         for (int j = 0; j< markers.cols; j++)
+//         {
+//             if(markers.IntPtr(i,j)[0] == -1)
+//             {
+//                 f.ucharPtr(i,j)[0] = 255; // R
+//                 f.ucharPtr(i,j)[1] = 0; // G
+//                 f.ucharPtr(i,j)[2] = 0; // B
+//             }
+//         }
+//     }
+// }
+
+//////
+
+cv::Mat img_sobel_abssobel(cv::Mat frame_crop, int ret = 0)
+{
+    //ret = 0 --> dstx
+    //ret = 1 --> dsty
+    //ret = 2 --> absDstx
+    //ret = 3 --> absDsty
+    // Image Sobel and Image AbsSobel
+    // https://docs.opencv.org/trunk/da/d85/tutorial_js_gradients.html
+    // compute image gradient on two different directions
+
+    cv::Mat f = frame_crop.clone();
+    int x, y;
+    (ret == 0 || ret == 2) ? x = 1, y = 0 : NULL;
+    (ret == 1 || ret == 3) ? x = 0, y = 1 : NULL;
+    cv::Mat dst;
+    cv::cvtColor(f, f, cv::COLOR_RGB2GRAY, 0);
+    // You can try more different parameters
+    cv::Sobel(f, dst, CV_8U, x, y, 3, 1, 0, cv::BORDER_DEFAULT);
+    // for absSobel
+    if (ret == 2 || ret == 3)
+        cv::convertScaleAbs(dst, dst, 1, 0);
+    // next 3 rows to be checked
+    //// ??cv::Mat f2 = frame_crop.clone();
+    //// cv.Scharr(?(f,f2), dstx, cv.CV_8U, 1, 0, 1, 0, cv.BORDER_DEFAULT);
+    //// cv.Scharr(?(f,f2), dsty, cv.CV_8U, 0, 1, 1, 0, cv.BORDER_DEFAULT);
+    return dst;
+}
+
+cv::Mat img_laplacian(cv::Mat frame_crop, int ret = 1)
+{
+    //ret = 0 --> src_gray
+    //ret = 1 --> dst
+    // Image Laplacian
+    // compute image gradient with laplacian
+    cv::Mat f = frame_crop.clone();
+    cv::Mat src_gray, dst;
+    int kernel_size = 3;
+    int scale = 1;
+    int delta = 0;
+    int ddepth = CV_16S;
+    cv::GaussianBlur(f, f, cv::Size(3, 3), 0, 0, cv::BORDER_DEFAULT);
+    /// Convert the image to grayscale
+    cv::cvtColor(f, src_gray, CV_RGB2GRAY);
+    if (ret == 0)
+        return src_gray;
+
+    // else: Apply Laplace function
+    cv::Mat abs_dst;
+    cv::Laplacian(src_gray, dst, ddepth, kernel_size, scale, delta, cv::BORDER_DEFAULT);
+    // //compute sharpness
+    // float sharpnessValue = cv::mean(dst);
+    return dst;
+}
+
+cv::Mat find_contours(cv::Mat frame_crop, cv::Mat img, cv::Mat canny_output, int n_lines = 1)
+{
+    // n_line: number of line to plot on image
+    cv::Mat img_line = frame_crop.clone();
+    cv::Mat ret_thresh;
+    std::vector<std::vector<cv::Point>> contours;
+    double thresh = 127;
+    double maxValue = 255;
+    cv::threshold(img, ret_thresh, thresh, maxValue, 0); //0); // = cv2.threshold(img,127,255,0)
+    cv::findContours(canny_output, contours, 1, 2);      //cv::CHAIN_APPROX_SIMPLE );//1, 2); //contours,hierarchy = cv2.findContours(thresh, 1, 2)
+    // cv::threshold(img2, ret2, thresh, maxValue, 0);
+    // cv::findContours(canny_output2, contours2, 1, 2);
+    // cv::threshold(img3a, ret3a, thresh, maxValue, 0);
+    // cv::findContours(canny_output3a, contours3a, 1, 2);
+    // cv::threshold(img3b, ret3b, thresh, maxValue, 0);
+    // cv::findContours(canny_output3b, contours3b, 1, 2);
+
+    cv::Vec4f line;
+    float vx, vy, x, y;
+    int lefty, righty;
+    for (int i = 0; i < n_lines; i++)
+    {
+        cv::fitLine(contours[i], line, CV_DIST_L2, 0, 0.01, 0.01);
+        vx = line(0);
+        vy = line(1);
+        x = line(2);
+        y = line(3);
+        lefty = int((-x * vy / vx) + y);
+        righty = int(((img.cols - x) * vy / vx) + y);
+        cv::line(img_line, cv::Point(img.cols - 1, righty), cv::Point(0, lefty), (255, 0, 0), 2);
+    }
+
+    // cv::imshow("bla", img);
+    // cv::waitKey(1000);
+    return img_line;
+}
+
+// cv::Mat fit_rectangular(cv::Mat frame_crop, cv::Mat img, cv::Mat canny_output)
+// {
+//     cv::Mat img_clone = frame_crop.clone();
+//     cv::Mat ret_thresh;
+//     std::vector<std::vector<cv::Point> > contours;
+//     double thresh = 127;
+//     double maxValue = 255;
+//     cv::threshold(img, ret_thresh, thresh, maxValue, 0);//0); // = cv2.threshold(img,127,255,0)
+//     cv::findContours(canny_output, contours, 1, 2);//cv::CHAIN_APPROX_SIMPLE );//1, 2); //contours,hierarchy = cv2.findContours(thresh, 1, 2)
+
+//     cv::RotatedRect rect = cv::minAreaRect(contours[0]);
+//     cv::Mat boxPts1;
+//     std::vector<std::vector<cv::Point> > boxPts2;
+//     cv::boxPoints(rect, boxPts1);
+//     // boxPts = np.int0(boxPts);
+//     for (int x = 0; x < img.cols; x++)
+//         for (int y = 0; y < img.rows; y++)
+//             boxPts2.at(x).push_back(cv::Point(boxPts1.at<int>(x, y)));
+
+//     cv::drawContours(img_clone, boxPts2,0,(0,0,255),2);
+//     // drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, Point() );
+//     return img_clone;
+// }
+
+cv::Mat compute_saliency(cv::Mat frame_crop, cv::Ptr<cv::saliency::Saliency> saliencyAlgorithm, int const_molt_mat, int ret = 0)
+{
+    //ret=0 --> saliencyMap
+    //ret=1 --> binaryMap
+    // SPECTRAL_RESIDUAL algorithm
+    cv::Mat f = frame_crop.clone();
+    cv::Mat saliencyMap;
+    cv::Mat binaryMap;
+
+    if (saliencyAlgorithm->computeSaliency(f, saliencyMap))
+    {
+        if (ret == 0)
+            return saliencyMap * const_molt_mat;
+
+        cv::saliency::StaticSaliencySpectralResidual spec;
+        spec.computeBinaryMap(saliencyMap, binaryMap);
+
+        // imshow( "Saliency Map", saliencyMap );
+        // imshow( "Original Image", image );
+        // imshow( "Binary Map", binaryMap );
+        // waitKey( 0 );
+        return binaryMap * const_molt_mat;
+    }
+    return cv::Mat(0, 0, CV_8U, cv::Scalar(0, 0, 0, 0));
+}
+
+//////
+
+void image_segmentation(cv::Mat frame_crop, int frame_nbr, int i)
+{
+    // Watershed Algorithm
+    // https://docs.opencv.org/3.4/d7/d1c/tutorial_js_watershed.html
+
+    auto step_t_segmentation = std::chrono::steady_clock::now();
+    auto end_t_segmentation = std::chrono::steady_clock::now();
+    cv::Mat ret;
+    // ret = img_threshold(frame_crop);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgthr.jpg", frame_nbr, i, img_threshold(frame_crop));
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME imgthr (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // ret =img_background(frame_crop);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgback.jpg", frame_nbr, i, img_background(frame_crop));
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME imgback (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // ret = img_dist_transform(frame_crop);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgtrans.jpg", frame_nbr, i, img_dist_transform(frame_crop));
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME imgtrans (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // // ret = img_watershed(frame_crop);
+    // if(SAVE) SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgwatershed.jpg", frame_nbr, i, img_watershed(frame_crop));
+}
+
+void image_gradients(cv::Mat frame_crop, int frame_nbr, int i)
+{
+    // Image Gradients
+    // https://docs.opencv.org/trunk/da/d85/tutorial_js_gradients.html
+
+    auto step_t_segmentation = std::chrono::steady_clock::now();
+    auto end_t_segmentation = std::chrono::steady_clock::now();
+    cv::Mat ret;
+    // sobel
+    // ret = img_sobel_abssobel(frame_crop, 0);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgsobel_x_8U.jpgg", frame_nbr, i, img_sobel_abssobel(frame_crop, 0));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME sobel0 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // ret = img_sobel_abssobel(frame_crop, 1);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgsobel_y_8U.jpg", frame_nbr, i, img_sobel_abssobel(frame_crop, 1));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME sobel1 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // ret = img_sobel_abssobel(frame_crop, 2);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgsobel_x_64F.jpg", frame_nbr, i, img_sobel_abssobel(frame_crop, 2));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME sobel2 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // ret = img_sobel_abssobel(frame_crop, 3);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imgsobel_y_64F.jpg", frame_nbr, i, img_sobel_abssobel(frame_crop, 3));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME sobel3 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // laplacian
+    // ret = img_laplacian(frame_crop, 0);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imglaplacian_gr.jpg", frame_nbr, i, img_laplacian(frame_crop, 0));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME laplacian0 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // ret = img_laplacian(frame_crop, 1);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_imglaplacian_dst.jpg", frame_nbr, i, img_laplacian(frame_crop, 1));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME laplacian1 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    //////
+}
+
+void image_find_contours(cv::Mat frame_crop, int frame_nbr, int i)
+{
+    // Finding contours in your image
+    // https://docs.opencv.org/3.4/df/d0d/tutorial_find_contours.html
+
+    auto step_t_segmentation = std::chrono::steady_clock::now();
+    auto end_t_segmentation = std::chrono::steady_clock::now();
+    // plot lines on figure. 3 ways:
+    // 1 - use gray image (no more operations) to get contours (one line)
+    // 2 - use laplacian image (one line)
+    // 3 - use sobel (1st dir) image and sobel (2nd dir) image to plot two different lines
+    cv::Mat canny_output1, canny_output2, canny_output3a, canny_output3b;
+    cv::Mat contours;
+    // src_gray
+    cv::Mat img1 = img_laplacian(frame_crop, 0);
+    cv::Canny(img1, canny_output1, 100, 100 * 2);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_canny1.jpg", frame_nbr, i, canny_output1);
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME canny1 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // // dst
+    // cv::Mat img2 = img_laplacian(frame_crop, 2);
+    // cv::Canny(img2, canny_output2, 100, 100*2 );
+    cv::Canny(img1, canny_output2, 100, 100 * 2);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_canny2.jpg", frame_nbr, i, canny_output2);
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME canny2 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // dstx
+    cv::Mat img3a = img_sobel_abssobel(frame_crop, 0);
+    cv::Canny(img3a, canny_output3a, 100, 100 * 2);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_canny3a.jpg", frame_nbr, i, canny_output3a);
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME canny3a (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // dsty
+    cv::Mat img3b = img_sobel_abssobel(frame_crop, 1);
+    cv::Canny(img3b, canny_output3b, 100, 100 * 2);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_canny3b.jpg", frame_nbr, i, canny_output3b);
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME canny3b (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // 1 line
+    // contours = find_contours(frame_crop, img1, canny_output1, 1);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_line1.jpg", frame_nbr, i, find_contours(frame_crop, img1, canny_output1, 1));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME line1 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // 3 line
+    // contours = find_contours(frame_crop, img1, canny_output2, 1);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_line2.jpg", frame_nbr, i, find_contours(frame_crop, img1, canny_output2, 1));
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME line2 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+    // mix 1 line of image with 1 line of another
+    cv::Mat img_line = frame_crop.clone();
+    img_line = find_contours(img_line, img3a, canny_output3a, 1);
+    img_line = find_contours(img_line, img3b, canny_output3b, 1);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_line3.jpg", frame_nbr, i, img_line);
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME line3 (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // img_line = frame_crop.clone();
+    // img_line = find_contours(img_line, img3a, canny_output3a, 2);
+    // img_line = find_contours(img_line, img3b, canny_output3b, 2);
+    // sprintf(buf_frame_crop_name,"../demo/demo/data/img_crop/%d_%d_line3bis.jpg",frame_nbr, i);
+    // cv::imwrite(buf_frame_crop_name, img_line);
+
+    // cv::Mat canny_output4;
+    // cv::Mat img4 = img_laplacian(frame_crop, 0);
+    // cv::Canny(img4, canny_output4, 100, 100*2 );
+    // printf(buf_frame_crop_name,"../demo/demo/data/img_crop/%d_%d_rect.jpg",frame_nbr, i);
+    // cv::imwrite(buf_frame_crop_name, fit_rectangular(frame_crop, img4, canny_output4));
+}
+
+void image_saliency(cv::Mat frame_crop, int frame_nbr, int i)
+{
+    // https://github.com/opencv/opencv_contrib/blob/master/modules/saliency/samples/computeSaliency.cpp
+    cv::Ptr<cv::saliency::Saliency> saliencyAlgorithm;
+
+    int const_molt_mat = 0;
+    auto step_t_segmentation = std::chrono::steady_clock::now();
+    auto end_t_segmentation = std::chrono::steady_clock::now();
+
+    // SPECTRAL_RESIDUAL
+    const_molt_mat = 255;
+    saliencyAlgorithm = cv::saliency::StaticSaliencySpectralResidual::create();
+    cv::Mat spect_res = compute_saliency(frame_crop, saliencyAlgorithm, const_molt_mat, 0);
+    if (!spect_res.empty())
+    {
+        if (SAVE)
+            SAVE_TO("../demo/demo/data/img_crop/%d_%d_saliency_SpectralResidual.jpg", frame_nbr, i, spect_res);
+    }
+    else
+    {
+        std::cout << "something is wrond (image_saliency)" << std::endl;
+    }
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME SPECTRAL_RESIDUAL (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // BINARY SPECTRAL_RESIDUAL
+    const_molt_mat = 255;
+    spect_res = compute_saliency(frame_crop, saliencyAlgorithm, const_molt_mat, 1);
+    if (!spect_res.empty())
+    {
+        if (SAVE)
+            SAVE_TO("../demo/demo/data/img_crop/%d_%d_saliency_BinarySpectralResidual.jpg", frame_nbr, i, spect_res);
+    }
+    else
+    {
+        std::cout << "something is wrond (image_saliency)" << std::endl;
+    }
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME BINARY SPECTRAL_RESIDUAL (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // FINE_GRAINED
+    const_molt_mat = 1;
+    saliencyAlgorithm = cv::saliency::StaticSaliencyFineGrained::create();
+    spect_res = compute_saliency(frame_crop, saliencyAlgorithm, const_molt_mat, 0);
+    if (!spect_res.empty())
+    {
+        if (SAVE)
+            SAVE_TO("../demo/demo/data/img_crop/%d_%d_saliency_FineGrained.jpg", frame_nbr, i, spect_res);
+    }
+    else
+    {
+        std::cout << "something is wrond (image_saliency)" << std::endl;
+    }
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME FINE_GRAINED (" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // saliencyAlgorithm = cv::saliency::ObjectnessBING::create();
+    // std::vector<cv::Vec4i> saliencyMap1;
+    // saliencyAlgorithm.dynamicCast<cv::saliency::ObjectnessBING>()->setTrainingPath( "" );
+    // saliencyAlgorithm.dynamicCast<cv::saliency::ObjectnessBING>()->setBBResDir( "Results" );
+    // std::cout<<"mmm"<<std::endl;
+    // saliencyAlgorithm->computeSaliency( frame_crop, saliencyMap1 );
+    // int ndet = int(saliencyMap1.size());
+    // std::cout << "Objectness done " << ndet << std::endl;
+    // // // The result are sorted by objectness. We only use the first maxd boxes here.
+    // // int maxd = 7, step = 255 / maxd, jitter=9; // jitter to seperate single rects
+    // // cv::Mat draw = frame_crop.clone();
+    // // for (int i = 0; i < std::min(maxd, ndet); i++)
+    // // {
+    // //     cv::Vec4i bb = saliencyMap1[i];
+    // //     cv::Scalar col = cv::Scalar(((i*step)%255), 100, 255-((i*step)%255));
+    // //     cv::Point off(cv::theRNG().uniform(-jitter,jitter), cv::theRNG().uniform(-jitter,jitter));
+    // //     cv::rectangle(draw, cv::Point(bb[0]+off.x, bb[1]+off.y), cv::Point(bb[2]+off.x, bb[3]+off.y), col, 2);
+    // //     cv::rectangle(draw, cv::Rect(20, 20+i*10, 10,10), col, -1); // mini temperature scale
+    // // }
+    // // imshow("BING", draw);
+    // // waitKey();
+    // printf(buf_frame_crop_name,"../demo/demo/data/img_crop/%d_%d_saliency_BING.jpg",frame_nbr, i);
+    // cv::imwrite(buf_frame_crop_name, saliencyMap1);
+
+    ////
+
+    // BING WANG APR 2014
+    cv::Mat saliencyMap;
+    cv::Mat frame_sal = frame_crop.clone();
+    saliencyAlgorithm = cv::saliency::MotionSaliencyBinWangApr2014::create();
+    saliencyAlgorithm.dynamicCast<cv::saliency::MotionSaliencyBinWangApr2014>()->setImagesize(frame_sal.cols, frame_sal.rows);
+    saliencyAlgorithm.dynamicCast<cv::saliency::MotionSaliencyBinWangApr2014>()->init();
+    cvtColor(frame_sal, frame_sal, cv::COLOR_BGR2GRAY);
+    saliencyAlgorithm->computeSaliency(frame_sal, saliencyMap);
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d_saliency_BinWangApr.jpg", frame_nbr, i, saliencyMap);
+
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " - TIME BING WANG APR 2014(" << frame_nbr << "-" << i << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+}
+
+cv::Mat frame_disparity(cv::Mat pre_frame, cv::Mat frame, int frame_nbr, int i, int ret = 0)
+{
+    // https://stackoverflow.com/questions/27035672/cv-extract-differences-between-two-images
+    cv::Mat backgroundImage = pre_frame.clone();
+    cv::Mat currentImage = frame.clone();
+    cv::Mat diffImage;
+    // pass to HSV color
+    if (ret)
+    {
+        cv::cvtColor(backgroundImage, backgroundImage, CV_BGR2HSV);
+        cv::cvtColor(currentImage, currentImage, CV_BGR2HSV);
+    }
+    cv::absdiff(backgroundImage, currentImage, diffImage);
+
+    cv::Mat foregroundMask = cv::Mat::zeros(diffImage.rows, diffImage.cols, CV_8UC1);
+    // std::cout<<"diffImage: "<<diffImage.cols<<" - "<<diffImage.rows<<std::endl;
+    // if(SAVE) SAVE_TO("../demo/demo/data/img_disparity/%d_%d_pc1.jpg", frame_nbr, i, backgroundImage);
+    // if(SAVE) SAVE_TO("../demo/demo/data/img_disparity/%d_%d_c1.jpg", frame_nbr, i, currentImage);
+    float threshold = 30.0f;
+    float dist;
+
+    for (int j = 0; j < diffImage.rows; ++j)
+    {
+        for (int k = 0; k < diffImage.cols; ++k)
+        {
+            cv::Vec3b pix = diffImage.at<cv::Vec3b>(j, k);
+
+            dist = (pix[0] * pix[0] + pix[1] * pix[1] + pix[2] * pix[2]);
+            dist = sqrt(dist);
+
+            if (dist > threshold)
+            {
+                foregroundMask.at<unsigned char>(j, k) = 255;
+            }
+        }
+    }
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_disparity/%d_%d_dif.jpg", frame_nbr, i, foregroundMask);
+
+    return foregroundMask;
+}
+
+void frame_box_disparity(cv::Mat pre_frame, cv::Mat frame, std::vector<cv::Rect> pre_rois, int frame_nbr)
+{
+
+    int roi_tollerance = 10;
+    cv::Mat pre_frame_crop, frame_crop;
+    int dx, dy;
+    int id = 1;
+    auto step_t_segmentation = std::chrono::steady_clock::now();
+    auto end_t_segmentation = std::chrono::steady_clock::now();
+
+    for (auto r : pre_rois)
+    {
+        if (SAVE)
+            SAVE_TO("../demo/demo/data/img_disparity/%d_%d_orig.jpg", frame_nbr, id, pre_frame(r));
+
+        //resize last roi with a tollerance
+        dx = r.width / roi_tollerance;
+        dy = r.height / roi_tollerance;
+        r.x = (r.x - dx > 0) ? (r.x - dx) : 0;
+        r.y = (r.y - dy > 0) ? (r.y - dy) : 0;
+        // std::cout<<"disp: x "<<r.x<<" - y "<<r.y<<std::endl;
+        r.width = ((r.x + r.width + dx + dx) >= frame.cols) ? (frame.cols - 1 - r.x) : (r.width + dx + dx);
+        r.height = ((r.y + r.height + dy + dy) >= frame.rows) ? (frame.rows - 1 - r.y) : (r.height + dy + dy);
+        // std::cout<<"disp: w "<<r.width<<" - h "<<r.height<<std::endl;
+        // std::cout<<"disp: wf "<<frame.cols<<" - hf "<<frame.rows<<std::endl;
+        // std::cout<<"---"<<std::endl;
+        // std::cout<<"disp: x "<<r.x<<" to "<<r.width+r.x<<" wf "<<frame.cols<<std::endl;
+        // std::cout<<"disp: y "<<r.y<<" to "<<r.height+r.y<<" hf "<<frame.rows<<std::endl;
+
+        //crop pre_frame and current frame
+        pre_frame_crop = pre_frame(r);
+        frame_crop = frame(r);
+        if (SAVE)
+            SAVE_TO("../demo/demo/data/img_disparity/%d_%d_cur.jpg", frame_nbr, id, frame_crop);
+        if (SAVE)
+            SAVE_TO("../demo/demo/data/img_disparity/%d_%d_pre.jpg", frame_nbr, id, pre_frame_crop);
+
+        // difference from two consecutive frame
+        step_t_segmentation = std::chrono::steady_clock::now();
+        frame_disparity(pre_frame_crop, frame_crop, frame_nbr, id, 0);
+        end_t_segmentation = std::chrono::steady_clock::now();
+        std::cout << " TIME frame_disparity (" << frame_nbr << "-" << id << ") : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+        step_t_segmentation = end_t_segmentation;
+        id++;
+    }
+}
+
+void segmentation(cv::Mat pre_frame, cv::Mat frame_crop, int frame_nbr, int i, int mode)
+{
+    //mode=0 (for whole frame), it computes the frame disparity
+    //mode=1 (for single box), it doesn't compute the frame disparity (it has already been done-see frame_box_disparity())
+    // whole figure
+    char buf_str[15];
+    if (!mode)
+        sprintf(buf_str, "whole frame");
+    else
+        sprintf(buf_str, "a box frame");
+
+    if (SAVE)
+        SAVE_TO("../demo/demo/data/img_crop/%d_%d.jpg", frame_nbr, i, frame_crop);
+
+    auto step_t_segmentation = std::chrono::steady_clock::now();
+    auto end_t_segmentation = std::chrono::steady_clock::now();
+
+    // Watershed Algorithm
+    std::cout << "image segmentation:" << std::endl;
+    image_segmentation(frame_crop, frame_nbr, i);
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " TIME " << buf_str << ": image_segmentation : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // Image Gradients
+    std::cout << "image gradients:" << std::endl;
+    image_gradients(frame_crop, frame_nbr, i);
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " TIME " << buf_str << ": image_gradients : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    // Find contours
+    std::cout << "image find contours:" << std::endl;
+    image_find_contours(frame_crop, frame_nbr, i);
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " TIME " << buf_str << ": image_find_contours : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    //saliency map
+    std::cout << "image saliency:" << std::endl;
+    image_saliency(frame_crop, frame_nbr, i);
+    end_t_segmentation = std::chrono::steady_clock::now();
+    std::cout << " TIME " << buf_str << ": image_saliency : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms" << std::endl;
+    step_t_segmentation = end_t_segmentation;
+
+    //frame disparity
+    if (!mode && frame_nbr != 0)
+    {
+        std::cout << "frame disparity:" << std::endl;
+        frame_disparity(pre_frame, frame_crop, frame_nbr, i, 0);
+        end_t_segmentation = std::chrono::steady_clock::now();
+        std::cout << " TIME " << buf_str << ": 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;
+    }
+}

src/class_src/calibration.cpp

@@ -0,0 +1,108 @@
+#include "calibration.h"
+
+void readTiff(char *filename, double *adfGeoTransform)
+{
+    GDALDataset *poDataset;
+    GDALAllRegister();
+    poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly);
+    if (poDataset != NULL)
+    {
+        poDataset->GetGeoTransform(adfGeoTransform);
+    }
+}
+
+void readCameraCalibrationYaml(const std::string &cameraCalib, cv::Mat &cameraMat, cv::Mat &distCoeff)
+{
+    YAML::Node config = YAML::LoadFile(cameraCalib);
+    const YAML::Node &node_test1 = config["camera_matrix"];
+
+    float data_cm[9];
+    for (std::size_t i = 0; i < node_test1["data"].size(); i++)
+        data_cm[i] = node_test1["data"][i].as<float>();
+    cv::Mat cameraMat_ = cv::Mat(3, 3, CV_32F, data_cm);
+    cameraMat = cameraMat_.clone();
+    std::cout << cameraMat << std::endl;
+    const YAML::Node &node_test2 = config["distortion_coefficients"];
+
+    float data_dc[5];
+    for (std::size_t i = 0; i < node_test2["data"].size(); i++)
+        data_dc[i] = node_test2["data"][i].as<float>();
+    cv::Mat distCoeff_ = cv::Mat(5, 1, CV_32F, data_dc);
+    distCoeff = distCoeff_.clone();
+    std::cout << distCoeff << std::endl;
+}
+
+void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform)
+{
+    //Returns global coordinates from pixel x, y coordinates
+    double xoff, a, b, yoff, d, e;
+    xoff = adfGeoTransform[0];
+    a = adfGeoTransform[1];
+    b = adfGeoTransform[2];
+    yoff = adfGeoTransform[3];
+    d = adfGeoTransform[4];
+    e = adfGeoTransform[5];
+
+    //printf("%f %f %f %f %f %f\n",xoff, a, b, yoff, d, e );
+
+    lon = a * x + b * y + xoff;
+    lat = d * x + e * y + yoff;
+}
+void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform)
+{
+    x = int(round((lon - adfGeoTransform[0]) / adfGeoTransform[1]));
+    y = int(round((lat - adfGeoTransform[3]) / adfGeoTransform[5]));
+}
+
+void fillMatrix(cv::Mat &H, double *matrix, bool show)
+{
+    double *vals = (double *)H.data;
+    for (int i = 0; i < 9; i++)
+    {
+        vals[i] = matrix[i];
+    }
+    if (show)
+        std::cout << H << "\n";
+}
+
+void read_projection_matrix(cv::Mat &H, char *path)
+{
+    FILE *fp;
+    char *line = NULL;
+    size_t len = 0;
+    ssize_t read;
+
+    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);
+
+    while ((read = getline(&line, &len, fp)) != -1)
+    {
+        std::cout << line << std::endl;
+        std::stringstream ss(line);
+        while (ss >> proj_matrix[i])
+            i++;
+    }
+    fclose(fp);
+    fillMatrix(H, proj_matrix);
+    free(line);
+}
+
+void convert_coords(std::vector<ObjCoords> &coords, int x, int y, int detected_class, cv::Mat H, double *adfGeoTransform)
+{
+    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
+    pixel2coord(ll[0].x, ll[0].y, latitude, longitude, adfGeoTransform);
+
+    ObjCoords coord;
+    coord.lat_ = latitude;
+    coord.long_ = longitude;
+    coord.class_ = detected_class;
+    coords.push_back(coord);
+}

src/class_src/configuration.cpp

@@ -0,0 +1,154 @@
+#include "configuration.h"
+
+/* Read the configuration file. The yaml file contains the information about the network, 
+ * the map and the cameras.
+**/
+void readCamerasParametersYaml(const std::string &camerasParams, Parameters_t *par)
+{
+
+    std::string cam, tmp;
+    YAML::Node config = YAML::LoadFile(camerasParams);
+    tmp = config["weights"].as<std::string>();//config["lastLogin"].as<DateTime>()
+    par->net = (char *) malloc((strlen(tmp.c_str())+1) * sizeof(char));
+    strcpy(par->net, tmp.c_str());
+    par->net[strlen(tmp.c_str())] = '\0';
+    // std::cout<<"- "<<par->net<<std::endl;  
+    tmp = config["tif_map"].as<std::string>();
+    par->tiffile = (char *) malloc((strlen(tmp.c_str())+1) * sizeof(char));
+    strcpy(par->tiffile, tmp.c_str());
+    par->tiffile[strlen(tmp.c_str())] = '\0';
+    // std::cout<<"- "<<par->tiffile<<std::endl;
+        
+    //read cameras infomrations
+    for (int i = 0; i<par->n_cameras; i++)
+    {
+        cam = std::to_string(par->cameras[i].CAM_IDX);
+        // std::cout<<"read "<<cam<<std::endl;
+        tmp = config[cam]["input_stream"].as<std::string>();
+        par->cameras[i].input = (char *) malloc((strlen(tmp.c_str())+1) * sizeof(char));
+        strcpy(par->cameras[i].input, tmp.c_str());
+        par->cameras[i].input[strlen(tmp.c_str())] = '\0';
+        // std::cout<<"- "<<par->cameras[i].input<<std::endl;
+        tmp = config[cam]["pmatrix"].as<std::string>();
+        par->cameras[i].pmatrix = (char *) malloc((strlen(tmp.c_str())+1) * sizeof(char));
+        strcpy(par->cameras[i].pmatrix, tmp.c_str());
+        par->cameras[i].pmatrix[strlen(tmp.c_str())] = '\0';
+        // std::cout<<"- "<<par->cameras[i].pmatrix<<std::endl;
+        tmp = config[cam]["maskfile"].as<std::string>();
+        par->cameras[i].maskfile = (char *) malloc((strlen(tmp.c_str())+1) * sizeof(char));
+        strcpy(par->cameras[i].maskfile, tmp.c_str());
+        par->cameras[i].maskfile[strlen(tmp.c_str())] = '\0';
+        // std::cout<<"- "<<par->cameras[i].maskfile<<std::endl;
+        tmp = config[cam]["cameraCalib"].as<std::string>();
+        par->cameras[i].cameraCalib = (char *) malloc((strlen(tmp.c_str())+1) * sizeof(char));
+        strcpy(par->cameras[i].cameraCalib, tmp.c_str());
+        par->cameras[i].cameraCalib[strlen(tmp.c_str())] = '\0';
+        // std::cout<<"- "<<par->cameras[i].cameraCalib<<std::endl;
+        tmp = config[cam]["maskFileOrient"].as<std::string>();
+        par->cameras[i].maskFileOrient = (char *) malloc((strlen(tmp.c_str())+1) * sizeof(char));
+        strcpy(par->cameras[i].maskFileOrient, tmp.c_str());
+        par->cameras[i].maskFileOrient[strlen(tmp.c_str())] = '\0';
+        // std::cout<<"- "<<par->cameras[i].maskFileOrient<<std::endl;
+    }
+}
+
+/* Read the parameters from the command line and configuration file.
+**/
+bool read_parameters(int argc, char *argv[], Parameters_t *par)
+{
+    bool no_params = false;
+    std::string params;// = "../../prova.yaml";
+    par->n_cameras = 0;
+    int opt;
+    char *help = "Yolo3 demo\nCommand:\n -i\tencrypted parameters file\n -n\tnumer of cameras\n \tlist of camera numbers (see -n)\n \tlist of flags for the visualization (see -n)\n\n";
+    // first read n_cameras parameter and/or help option 
+    while((opt = getopt(argc, argv, ":n:i:h")) != -1)
+    {
+        switch(opt)
+        {
+            case 'n': //number of cameras
+                std::cout<<"number of cameras"<<std::endl;
+                par->n_cameras = atoi(optarg);
+                break;
+            case 'h':
+                std::cout<<"help"<<std::endl;
+                std::cout<<help<<std::endl;
+                return false;
+            case 'i':
+                    std::cout<<"input parameters file"<<std::endl;
+                    params = optarg;
+                    std::cout<<"file: "<<params<<std::endl;
+                    break;
+            case ':':
+                printf("option needs a value\n");
+                no_params = true;
+                par->n_cameras = 1;
+                break;
+            case '?':
+                printf("unknown option: %c\n", optopt);
+                break;
+        }
+    }
+    par->cameras = (Camera_t *) malloc(par->n_cameras * sizeof(Camera_t)); 
+    bool *to_show = (bool *) malloc(par->n_cameras * sizeof(bool)); 
+    if(no_params) 
+    {
+        par->net = "yolo3_coco4.rt";
+        par->tiffile = "../demo/demo/data/map_b.tif";
+        par->cameras[0].CAM_IDX = 20936;
+        par->cameras[0].input = (char *)"../demo/demo/data/single_ped_2.mp4";
+        par->cameras[0].pmatrix = (char *)"../demo/demo/data/pmundist.txt";
+        par->cameras[0].maskfile = (char *)"../demo/demo/data/mask36.jpg";
+        par->cameras[0].cameraCalib = (char *)"../demo/demo/data/calib36.params";
+        par->cameras[0].maskFileOrient = (char *)"../demo/demo/data/mask_orient/6315_mask_orient.jpg";
+        par->cameras[0].to_show = true;
+        to_show[0] = true;
+    }
+    else
+    {    
+        int i = 0;
+        for(; optind < argc; optind++)
+        {
+            if(i==par->n_cameras)
+                break;
+            printf("extra arguments: %s\n", argv[optind]);
+            par->cameras[i].CAM_IDX = atoi(argv[optind]);
+            i++;
+        }
+        i=0;
+        for(; optind < argc; optind++)
+        {
+            if(i==par->n_cameras)
+                break;
+            printf("extra arguments: %s\n", argv[optind]);
+            par->cameras[i].to_show = atoi(argv[optind]); //only one camera can be shown
+            to_show[i] = par->cameras[i].to_show;
+            i++;
+        }
+        // TODO: now only one camera can be visualized
+        int check_visualization=0;
+        for(int i = 0; i<par->n_cameras; i++)
+            check_visualization += to_show[i];
+        if(check_visualization > 1)
+            return false;
+        //decrypt the input file, save it in tmp directory
+        char s[200] = "";
+        strcat(s, "openssl enc -aes-256-cbc -d -in ");
+        strcat(s, params.c_str());
+        strcat(s, " -base64 -md sha1 -out /tmp/decrypt.yaml");
+        if(system(s))
+        {
+            fprintf(stderr, "Error system\n");
+            return false;
+        }
+        // read the file yaml to get the camera parameters
+        readCamerasParametersYaml("/tmp/decrypt.yaml", par);
+        // delete the file decrypted
+        if (system("rm /tmp/decrypt.yaml"))
+        {
+            fprintf(stderr, "Error system\n");
+            return false;
+        }
+    }
+    return true;
+}

src/class_src/message.cpp

@@ -0,0 +1,136 @@
+#include "message.h"
+#include "calibration.h"
+
+unsigned long long time_in_ms()
+{
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000;
+    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();
+    m->objects.clear();
+    double lat, lon, alt;
+
+    for (auto t : trackers)
+    {
+        if (t.pred_list_.size() > 0)
+        {
+            Categories cat;
+            switch (t.class_)
+            {
+            case 0:
+                cat = Categories::C_person;
+                break;
+            case 1:
+                cat = Categories::C_car;
+                break;
+            case 2:
+                cat = Categories::C_car;
+                break;
+            case 3:
+                cat = Categories::C_bus;
+                break;
+            case 4:
+                cat = Categories::C_motorbike;
+                break;
+            case 5:
+                cat = Categories::C_bycicle;
+                break;
+            }
+            //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);
+
+            // 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;
+            // }
+
+            // 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 = 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 = 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;
+            m->objects.push_back(r);
+        }
+    }
+    m->num_objects = m->objects.size();
+}