Changes tirlat code

code/trilateration.cpp

@@ -12,7 +12,7 @@ namespace Trilateration
 {
     // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp
 
-    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
+    Point2 peusodInverse2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
     {
         // To locate position on a 2d plan, have to get at least 3 becaons,
         // otherwise return false.
@@ -53,7 +53,7 @@ namespace Trilateration
         return Point2(pseudoInv.x(), pseudoInv.y());
     }
 
-    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
+    Point3 pseudoInverse3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
     {
         // To locate position in a 3D space, have to get at least 4 becaons
         if (positions.size() < 4)
@@ -158,33 +158,76 @@ namespace Trilateration
         {}
     };
 
+    std::string lmStatusToString(const Eigen::LevenbergMarquardtSpace::Status stat)
+    {
+        switch (stat)
+        {
+        // Non-Erros
+        case Eigen::LevenbergMarquardtSpace::NotStarted:
+            return "Not started. No Error.";
+        case Eigen::LevenbergMarquardtSpace::Running:
+            return "Running. No Error.";
+        // Error
+        case Eigen::LevenbergMarquardtSpace::ImproperInputParameters:
+            return "Error. Invalid input parameters";
+        case Eigen::LevenbergMarquardtSpace::UserAsked:
+            return "Error in user-implemented evaluation or gradient computation.";
+        // Warnings
+        case Eigen::LevenbergMarquardtSpace::RelativeReductionTooSmall:
+            return "WARN: The cosine of the angle between fvec and any column of the jacobian is at most gtol in absolute value.";
+        case Eigen::LevenbergMarquardtSpace::RelativeErrorTooSmall:
+            return "WARN: Relative error too small.";
+        case Eigen::LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall:
+            return "WARN: Relative error and reduction too small.";
+        case Eigen::LevenbergMarquardtSpace::CosinusTooSmall:
+            return "WARN: The cosine of the angle between fvec and any column of the jacobian is at most gtol in absolute value.";
+        case Eigen::LevenbergMarquardtSpace::TooManyFunctionEvaluation:
+            return "WARN: Too many function evaluations done.";
+        case Eigen::LevenbergMarquardtSpace::FtolTooSmall:
+            return "WARN: ftol is too small. No further reduction in the sum of squares is possible";
+        case Eigen::LevenbergMarquardtSpace::XtolTooSmall:
+            return "WARN: xtol is too small. No further improvement in the approximate solution x is possible.";
+        case Eigen::LevenbergMarquardtSpace::GtolTooSmall:
+            return "WARN: gtol is too small.fvec is orthogonal to the columns of the jacobian to machine precision.";
+        default:
+            return "Unkown status. Status=" + std::to_string(static_cast<int>(stat));
+        }
+    }
+
     Point2 levenbergMarquardt(const std::vector<Point2>& positions, const std::vector<float>& distances)
     {
-        Point2 pseudoInvApprox = calculateLocation2d(positions, distances);
+        const Point2 pseudoInvApprox = peusodInverse2d(positions, distances);
-        Eigen::Vector2d initVal;
-        initVal << pseudoInvApprox.x, pseudoInvApprox.y;
 
         Eigen::Vector2d startVal;
-        //startVal << pseudoInvApprox.x, pseudoInvApprox.y;
+        startVal << pseudoInvApprox.x, pseudoInvApprox.y;
-        startVal << 0, 0;
+        //startVal << 0, 0;
 
         DistanceFunction functor(positions, distances);
         DistanceFunctionDiff numDiff(functor);
         Eigen::LevenbergMarquardt<DistanceFunctionDiff, double> lm(numDiff);
-        lm.parameters.maxfev = 2000;
+
+        // Parameters
+        // factor : Sets the step bound for the diagonal shift
+        // maxfev : Sets the maximum number of function evaluation
+        // ftol   : Sets the tolerance for the norm of the vector function
+        // xtol   : Sets the tolerance for the norm of the solution vector
+        // gtol   : Sets the tolerance for the norm of the gradient of the error vector
+        // epsilon: Sets the error precision
+        lm.parameters.maxfev = 200;
         lm.parameters.xtol = 1.0e-10;
-        std::cout << lm.parameters.maxfev << std::endl;
+        
 
         Eigen::VectorXd z = startVal;
-        int ret = lm.minimize(z);
+        auto status = lm.minimize(z);
-        std::cout << "iter count: " << lm.iter << std::endl;
+        std::cout << "Levenberg Marquardt" << std::endl;
-        std::cout << "return status: " << ret << std::endl;
+        std::cout << "status: " << lmStatusToString(status) << std::endl;
-        std::cout << "zSolver: " << z.transpose() << std::endl;
+        std::cout << "iter count: " << lm.iter << std::endl;        
-        std::cout << "pseudoInv: " << initVal.transpose() << std::endl;
+        std::cout << "levenMarq: " << z.transpose() << std::endl;
+        std::cout << "pseudoInv: " << startVal.transpose() << std::endl;
 
+        const Point2 levenPos(z(0), z(1));
 
-        Point2 bla(z(0), z(1));
+        // Compute error        
-
         double errPseudo = 0;
         double errLeven = 0;
         for (size_t i = 0; i < positions.size(); i++)
@@ -192,7 +235,7 @@ namespace Trilateration
             double d1 = pseudoInvApprox.getDistance(positions[i]) - distances[i];
             errPseudo += d1 * d1;
 
-            double d2 = bla.getDistance(positions[i]) - distances[i];
+            double d2 = levenPos.getDistance(positions[i]) - distances[i];
             errLeven += d2 * d2;
         }
 
@@ -201,7 +244,6 @@ namespace Trilateration
         std::cout << "err pseud: " << errPseudo << std::endl;
         std::cout << "err leven: " << errLeven << std::endl << std::endl;
 
-        return Point2(z(0), z(1));
+        return levenPos;
-
     }
 }

code/trilateration.h

@@ -7,8 +7,8 @@
 
 namespace Trilateration
 {
-    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances);
+    Point2 peusodInverse2d(const std::vector<Point2>& positions, const std::vector<float>& distances);
-    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances);
+    Point3 pseudoInverse3d(const std::vector<Point3>& positions, const std::vector<float>& distances);
 
     Point2 levenbergMarquardt(const std::vector<Point2>& positions, const std::vector<float>& distances);
 };