diff --git a/code/trilateration.cpp b/code/trilateration.cpp index 94bfd9d..5a91e03 100644 --- a/code/trilateration.cpp +++ b/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& positions, const std::vector& distances) + Point2 peusodInverse2d(const std::vector& positions, const std::vector& 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& positions, const std::vector& distances) + Point3 pseudoInverse3d(const std::vector& positions, const std::vector& 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(stat)); + } + } + Point2 levenbergMarquardt(const std::vector& positions, const std::vector& distances) { - Point2 pseudoInvApprox = calculateLocation2d(positions, distances); - Eigen::Vector2d initVal; - initVal << pseudoInvApprox.x, pseudoInvApprox.y; + const Point2 pseudoInvApprox = peusodInverse2d(positions, distances); Eigen::Vector2d startVal; - //startVal << pseudoInvApprox.x, pseudoInvApprox.y; - startVal << 0, 0; + startVal << pseudoInvApprox.x, pseudoInvApprox.y; + //startVal << 0, 0; DistanceFunction functor(positions, distances); DistanceFunctionDiff numDiff(functor); Eigen::LevenbergMarquardt 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); - std::cout << "iter count: " << lm.iter << std::endl; - std::cout << "return status: " << ret << std::endl; - std::cout << "zSolver: " << z.transpose() << std::endl; - std::cout << "pseudoInv: " << initVal.transpose() << std::endl; + auto status = lm.minimize(z); + std::cout << "Levenberg Marquardt" << std::endl; + std::cout << "status: " << lmStatusToString(status) << std::endl; + std::cout << "iter count: " << lm.iter << 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; } } \ No newline at end of file diff --git a/code/trilateration.h b/code/trilateration.h index 3618dff..772afd6 100644 --- a/code/trilateration.h +++ b/code/trilateration.h @@ -7,8 +7,8 @@ namespace Trilateration { - Point2 calculateLocation2d(const std::vector& positions, const std::vector& distances); - Point3 calculateLocation3d(const std::vector& positions, const std::vector& distances); + Point2 peusodInverse2d(const std::vector& positions, const std::vector& distances); + Point3 pseudoInverse3d(const std::vector& positions, const std::vector& distances); Point2 levenbergMarquardt(const std::vector& positions, const std::vector& distances); };