Added pose estimation 3D2D 2D2D and triangulation. Now going for the bundle adjustement and test...