FHWS/FtmPrologic
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Changes tirlat code

Browse Source
This commit is contained in:
Markus Bullmann
2019-12-04 16:14:22 +01:00
parent dcdaa78133
commit 22f6c720e5
2 changed files with 63 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
code/trilateration.cpp
View File

@@ -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);
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<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 << "Levenberg Marquardt" << std::endl;
std::cout << "status: " << lmStatusToString(status) << std::endl;
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;
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;
}
}

4
code/trilateration.h
View File

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