diff --git a/MathLib/MathTools.cpp b/MathLib/MathTools.cpp
index bf421e98986b47c2e62e87fb7617d5af0360a3b6..5d7017408e0e27d0471fc812588c52c4f8b96509 100644
--- a/MathLib/MathTools.cpp
+++ b/MathLib/MathTools.cpp
@@ -20,12 +20,10 @@ namespace MathLib
 double calcProjPntToLineAndDists(Point3d const& pp, Point3d const& pa,
                                  Point3d const& pb, double& lambda, double& d0)
 {
-    auto const a =
-        Eigen::Map<Eigen::Vector3d>(const_cast<double*>(pa.getCoords()));
-    auto const b =
-        Eigen::Map<Eigen::Vector3d>(const_cast<double*>(pb.getCoords()));
-    auto const p =
-        Eigen::Map<Eigen::Vector3d>(const_cast<double*>(pp.getCoords()));
+    auto const a = Eigen::Map<Eigen::Vector3d const>(pa.getCoords());
+    auto const b = Eigen::Map<Eigen::Vector3d const>(pb.getCoords());
+    auto const p = Eigen::Map<Eigen::Vector3d const>(pp.getCoords());
+
     // g(lambda) = a + lambda v, v = b-a
     Eigen::Vector3d const v = b - a;