diff --git a/demo/demo/data/calib_cameras/10218.params b/demo/demo/data/calib_cameras/10218.params
new file mode 100644
index 0000000..8f42f83
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/20932.params b/demo/demo/data/calib_cameras/20932.params
new file mode 100644
index 0000000..9dd8af4
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/20936.params b/demo/demo/data/calib_cameras/20936.params
new file mode 100644
index 0000000..4e4d97b
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/20937.params b/demo/demo/data/calib_cameras/20937.params
new file mode 100644
index 0000000..2504f27
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/20939.params b/demo/demo/data/calib_cameras/20939.params
new file mode 100644
index 0000000..ee94c14
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/20940.params b/demo/demo/data/calib_cameras/20940.params
new file mode 100644
index 0000000..ceb784c
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/6310.params b/demo/demo/data/calib_cameras/6310.params
new file mode 100644
index 0000000..9ed059a
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/6311.params b/demo/demo/data/calib_cameras/6311.params
new file mode 100644
index 0000000..ad1e6af
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/6312.params b/demo/demo/data/calib_cameras/6312.params
new file mode 100644
index 0000000..00087a1
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/6313.params b/demo/demo/data/calib_cameras/6313.params
new file mode 100644
index 0000000..6d34d08
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/6314.params b/demo/demo/data/calib_cameras/6314.params
new file mode 100644
index 0000000..6f3f951
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/6315.params b/demo/demo/data/calib_cameras/6315.params
new file mode 100644
index 0000000..ccb29c0
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/632.params b/demo/demo/data/calib_cameras/632.params
new file mode 100644
index 0000000..794d5e4
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/634.params b/demo/demo/data/calib_cameras/634.params
new file mode 100644
index 0000000..d762b81
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/calib_cameras/637.params b/demo/demo/data/calib_cameras/637.params
new file mode 100644
index 0000000..cfe17c9
--- /dev/null
+++ b/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
diff --git a/demo/demo/data/map/map_geo.jpg b/demo/demo/data/map/map_geo.jpg
new file mode 100644
index 0000000..3040221
Binary files /dev/null and b/demo/demo/data/map/map_geo.jpg differ
diff --git a/demo/demo/data/masks/10218_mask.jpg b/demo/demo/data/masks/10218_mask.jpg
new file mode 100644
index 0000000..4940aff
Binary files /dev/null and b/demo/demo/data/masks/10218_mask.jpg differ
diff --git a/demo/demo/data/masks/20932_mask.jpg b/demo/demo/data/masks/20932_mask.jpg
new file mode 100644
index 0000000..7f5d327
Binary files /dev/null and b/demo/demo/data/masks/20932_mask.jpg differ
diff --git a/demo/demo/data/masks/20936_mask.jpg b/demo/demo/data/masks/20936_mask.jpg
new file mode 100644
index 0000000..baa17f7
Binary files /dev/null and b/demo/demo/data/masks/20936_mask.jpg differ
diff --git a/demo/demo/data/masks/20937_mask.jpg b/demo/demo/data/masks/20937_mask.jpg
new file mode 100644
index 0000000..230e77f
Binary files /dev/null and b/demo/demo/data/masks/20937_mask.jpg differ
diff --git a/demo/demo/data/masks/20939_mask.jpg b/demo/demo/data/masks/20939_mask.jpg
new file mode 100644
index 0000000..8b71c82
Binary files /dev/null and b/demo/demo/data/masks/20939_mask.jpg differ
diff --git a/demo/demo/data/masks/20940_mask.jpg b/demo/demo/data/masks/20940_mask.jpg
new file mode 100644
index 0000000..0f63995
Binary files /dev/null and b/demo/demo/data/masks/20940_mask.jpg differ
diff --git a/demo/demo/data/masks/6310_mask.jpg b/demo/demo/data/masks/6310_mask.jpg
new file mode 100644
index 0000000..8047d7c
Binary files /dev/null and b/demo/demo/data/masks/6310_mask.jpg differ
diff --git a/demo/demo/data/masks/6311_mask.jpg b/demo/demo/data/masks/6311_mask.jpg
new file mode 100644
index 0000000..ee793eb
Binary files /dev/null and b/demo/demo/data/masks/6311_mask.jpg differ
diff --git a/demo/demo/data/masks/6312_mask.jpg b/demo/demo/data/masks/6312_mask.jpg
new file mode 100644
index 0000000..45ed617
Binary files /dev/null and b/demo/demo/data/masks/6312_mask.jpg differ
diff --git a/demo/demo/data/masks/6313_mask.jpg b/demo/demo/data/masks/6313_mask.jpg
new file mode 100644
index 0000000..ed471ab
Binary files /dev/null and b/demo/demo/data/masks/6313_mask.jpg differ
diff --git a/demo/demo/data/masks/6314_mask.jpg b/demo/demo/data/masks/6314_mask.jpg
new file mode 100644
index 0000000..139a0bb
Binary files /dev/null and b/demo/demo/data/masks/6314_mask.jpg differ
diff --git a/demo/demo/data/masks/6315_mask.jpg b/demo/demo/data/masks/6315_mask.jpg
new file mode 100644
index 0000000..7792157
Binary files /dev/null and b/demo/demo/data/masks/6315_mask.jpg differ
diff --git a/demo/demo/data/masks/634_mask.jpg b/demo/demo/data/masks/634_mask.jpg
new file mode 100644
index 0000000..4c3e073
Binary files /dev/null and b/demo/demo/data/masks/634_mask.jpg differ
diff --git a/demo/demo/data/masks/636_mask.jpg b/demo/demo/data/masks/636_mask.jpg
new file mode 100644
index 0000000..916b552
Binary files /dev/null and b/demo/demo/data/masks/636_mask.jpg differ
diff --git a/demo/demo/data/masks/637_mask.jpg b/demo/demo/data/masks/637_mask.jpg
new file mode 100644
index 0000000..72ea958
Binary files /dev/null and b/demo/demo/data/masks/637_mask.jpg differ
diff --git a/demo/demo/data/pmat_geo/pmat_geo_10218.txt b/demo/demo/data/pmat_geo/pmat_geo_10218.txt
new file mode 100644
index 0000000..46e0f5f
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_20932.txt b/demo/demo/data/pmat_geo/pmat_geo_20932.txt
new file mode 100644
index 0000000..253ccc2
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_20936.txt b/demo/demo/data/pmat_geo/pmat_geo_20936.txt
new file mode 100644
index 0000000..c86d20a
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_20937.txt b/demo/demo/data/pmat_geo/pmat_geo_20937.txt
new file mode 100644
index 0000000..e0c9986
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_20939.txt b/demo/demo/data/pmat_geo/pmat_geo_20939.txt
new file mode 100644
index 0000000..ca66b9c
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_20940.txt b/demo/demo/data/pmat_geo/pmat_geo_20940.txt
new file mode 100644
index 0000000..de433f4
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_6310.txt b/demo/demo/data/pmat_geo/pmat_geo_6310.txt
new file mode 100644
index 0000000..76cb6a3
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_6311.txt b/demo/demo/data/pmat_geo/pmat_geo_6311.txt
new file mode 100644
index 0000000..a239061
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_6312.txt b/demo/demo/data/pmat_geo/pmat_geo_6312.txt
new file mode 100644
index 0000000..5b538f9
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_6313.txt b/demo/demo/data/pmat_geo/pmat_geo_6313.txt
new file mode 100644
index 0000000..7c0cd90
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_6314.txt b/demo/demo/data/pmat_geo/pmat_geo_6314.txt
new file mode 100644
index 0000000..b351fb3
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_6315.txt b/demo/demo/data/pmat_geo/pmat_geo_6315.txt
new file mode 100644
index 0000000..577cb54
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_634.txt b/demo/demo/data/pmat_geo/pmat_geo_634.txt
new file mode 100644
index 0000000..da64d60
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_636.txt b/demo/demo/data/pmat_geo/pmat_geo_636.txt
new file mode 100644
index 0000000..2f659a9
--- /dev/null
+++ b/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 
diff --git a/demo/demo/data/pmat_geo/pmat_geo_637.txt b/demo/demo/data/pmat_geo/pmat_geo_637.txt
new file mode 100644
index 0000000..24b8022
--- /dev/null
+++ b/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 
diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp
index fbfc6b3..5472bc1 100644
--- a/demo/demo/demo.cpp
+++ b/demo/demo/demo.cpp
@@ -78,7 +78,7 @@ int main(int argc, char *argv[])
         maskfile = argv[7];
     char *cameraCalib = "../demo/demo/data/calib36.params";
     if (argc > 8)
-        cameraCalib = argv[7];
+        cameraCalib = argv[8];
 
     tk::dnn::Yolo3Detection yolo;
     yolo.init(net);
@@ -103,7 +103,7 @@ int main(int argc, char *argv[])
     {
         cv::namedWindow("detection", cv::WINDOW_NORMAL);
         cv::namedWindow("topview", cv::WINDOW_NORMAL);
-        frame_top = cv::imread("../demo/demo/data/map_b.jpg");
+        frame_top = cv::imread("../demo/demo/data/map/map_geo.jpg");
         original_frame_top = frame_top.clone();
     }
 
@@ -147,6 +147,13 @@ int main(int argc, char *argv[])
 
     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);*/
+    
+
     while (gRun)
     {
 
@@ -154,7 +161,7 @@ int main(int argc, char *argv[])
 
         cv::Mat temp = frame.clone();
         undistort(temp, frame, cameraMat, distCoeff);
-        //cv::imwrite("undistorted.jpg", frame);
+        cv::imwrite(std::to_string(CAM_IDX) + ".jpg", frame);
 
         if (!frame.data)
         {
@@ -195,6 +202,8 @@ int main(int argc, char *argv[])
 
                 if (objClass < 6)
                 {
+                    
+                    
                     convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr);
 
                     //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
@@ -210,6 +219,8 @@ int main(int argc, char *argv[])
             }
         }
 
+        TIMER_START
+
         //convert from latitude and longitude to meters for ekf
         cur_frame.clear();
         for (size_t i = 0; i < coords.size(); i++)
@@ -227,6 +238,7 @@ int main(int argc, char *argv[])
             Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
         }
 
+        TIMER_STOP
         std::cout << "There are " << trackers.size() << " trackers" << std::endl;
 
         if (to_show)
@@ -268,12 +280,16 @@ int main(int argc, char *argv[])
             }
         }
 
+
+
         if (to_show)
         {
             sem.lock();
             frame_v = frame.clone();
             frame_top_v = frame_top.clone();
             sem.unlock();
+
+            //outputVideo<< frame_top;
         }
 
         if (frame_nbr == 0 && to_show)
@@ -287,11 +303,8 @@ int main(int argc, char *argv[])
 
         frame_nbr++;
         prepare_message(m, coords, CAM_IDX);
-        std::stringbuf s;
-        Comm.serialize_coords(m, &s);
-        //std::cout<<s.str()<<std::endl;
-        //std::cout<<s.str().length()<<std::endl;
         Comm.send_message(m);
+
     }
 
     free(adfGeoTransform);
diff --git a/include/classutils.h b/include/classutils.h
index c300f9a..ce21f6b 100644
--- a/include/classutils.h
+++ b/include/classutils.h
@@ -76,6 +76,8 @@ void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform
 
     lon = a * x + b * y + xoff;
     lat = d * x + e * y + yoff;
+
+    
 }
 void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform)
 {
@@ -174,7 +176,7 @@ unsigned long long time_in_ms()
     return t_stamp_ms;
 }
 
-void prepare_message(Message *m, const std::vector<ObjCoords>& coords, int idx)
+void prepare_message(Message *m, const std::vector<ObjCoords> &coords, int idx)
 {
     m->cam_idx = idx;
     m->t_stamp_ms = time_in_ms();
@@ -184,6 +186,7 @@ void prepare_message(Message *m, const std::vector<ObjCoords>& coords, int idx)
     for (int i = 0; i < coords.size(); i++)
     {
         Categories cat;
+
         switch (coords[i].class_)
         {
         case 0:
@@ -205,7 +208,8 @@ void prepare_message(Message *m, const std::vector<ObjCoords>& coords, int idx)
             cat = Categories::C_bycicle;
             break;
         }
-        RoadUser r{coords[i].lat_, coords[i].long_, 0, 1, C_car};
+        RoadUser r{static_cast<float>(coords[i].lat_), static_cast<float>(coords[i].long_), 0, 1, cat};
+        std::cout<<std::setprecision(10) <<r.latitude<<" , "<<r.longitude << " " << cat << std::endl;
         m->objects.push_back(r);
     }