Replace Eigen with MathGeoLib