MADE:
--> Fix in Geometrics.cpp
--> Testing new rotation Matrix function
Por ahora, las matrices tienen sentido.... Puede que el fallo esté en el modelo pinhole. Revisar
Cant find rot error... everything to seem alright
--> split operations in order to simplify debugging.
--> Ori seems to be right, EKF seem to be right.... What about focal length???? Now using vicon operations and centroids of target in image to determine the focal length without the camera but its results
Think solver trouble with EKF and vicon data.... Rotations are euler XYZ but not R=RxRyRz but R=RzRyRx
MADE:
--> StereoEKF seems to be right as groundEKF. Only tested with one dataset.

TODO
--> Test with other datasets
Fixed error in git ignore with Tests/test_stereoEKF/prj/vs13/
Some fixes in TestSegmentation. And changed to dataset 2