Program Listing for File levmarq.h
↰ Return to documentation for file (/tmp/ws/src/aruco_ros/aruco/include/aruco/levmarq.h)
#ifndef ARUCO_MM__LevMarq_H
#define ARUCO_MM__LevMarq_H
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <functional>
#include <iostream>
#include <cmath>
#include <ctime>
#include <cstring>
#include <vector>
#include <chrono>
#include <iomanip>
namespace aruco
{
// Levenberg-Marquardt method for general problems Inspired in
//@MISC\{IMM2004-03215,
// author = "K. Madsen and H. B. Nielsen and O. Tingleff",
// title = "Methods for Non-Linear Least Squares Problems (2nd ed.)",
// year = "2004",
// pages = "60",
// publisher = "Informatics and Mathematical Modelling, Technical University of Denmark, {DTU}",
// address = "Richard Petersens Plads, Building 321, {DK-}2800 Kgs. Lyngby",
// url = "http://www.ltu.se/cms_fs/1.51590!/nonlinear_least_squares.pdf"
//}
template <typename T>
class LevMarq
{
public:
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> eVector;
typedef std::function<void(const eVector &, eVector &)> F_z_x;
typedef std::function<void(const eVector &, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &)> F_z_J;
LevMarq();
LevMarq(int maxIters, double minError, double min_step_error_diff = 0, double tau = 1,
double der_epsilon = 1e-3);
void setParams(int maxIters, double minError, double min_step_error_diff = 0,
double tau = 1, double der_epsilon = 1e-3);
double solve(eVector &z, F_z_x, F_z_J);
void init(eVector &z, F_z_x);
bool step(F_z_x f_z_x, F_z_J f_z_J);
bool step(F_z_x f_z_x);
double getCurrentSolution(eVector &z);
double getBestSolution(eVector &z);
double solve(eVector &z, F_z_x);
// to enable verbose mode
bool &verbose()
{
return _verbose;
}
// sets a callback func call at each step
void setStepCallBackFunc(std::function<void(const eVector &)> callback)
{
_step_callback = callback;
}
// sets a function that indicates when the algorithm must be stop. returns true if must stop and false otherwise
void setStopFunction(std::function<bool(const eVector &)> stop_function)
{
_stopFunction = stop_function;
}
void calcDerivates(const eVector &z, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &, F_z_x);
private:
int _maxIters;
double _minErrorAllowed, _der_epsilon, _tau, _min_step_error_diff;
bool _verbose;
//--------
eVector curr_z, x64;
double currErr, prevErr, minErr;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> I, J;
double mu, v;
std::function<void(const eVector &)> _step_callback;
std::function<bool(const eVector &)> _stopFunction;
};
template <typename T>
LevMarq<T>::LevMarq()
{
_maxIters = 1000;
_minErrorAllowed = 0;
_der_epsilon = 1e-3;
_verbose = false;
_tau = 1;
v = 5;
_min_step_error_diff = 0;
}
template <typename T>
LevMarq<T>::LevMarq(int maxIters, double minError, double min_step_error_diff, double tau,
double der_epsilon)
{
_maxIters = maxIters;
_minErrorAllowed = minError;
_der_epsilon = der_epsilon;
_verbose = false;
_tau = tau;
v = 5;
_min_step_error_diff = min_step_error_diff;
}
template <typename T>
void LevMarq<T>::setParams(int maxIters, double minError, double min_step_error_diff,
double tau, double der_epsilon)
{
_maxIters = maxIters;
_minErrorAllowed = minError;
_der_epsilon = der_epsilon;
_tau = tau;
_min_step_error_diff = min_step_error_diff;
}
template <typename T>
void LevMarq<T>::calcDerivates(const eVector &z,
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &j, F_z_x f_z_x)
{
for (int i = 0; i < z.rows(); i++)
{
eVector zp(z), zm(z);
zp(i) += _der_epsilon;
zm(i) -= _der_epsilon;
eVector xp, xm;
f_z_x(zp, xp);
f_z_x(zm, xm);
eVector dif = (xp - xm) / (2.f * _der_epsilon);
j.middleCols(i, 1) = dif;
}
}
template <typename T>
double LevMarq<T>::solve(eVector &z, F_z_x f_z_x)
{
return solve(z, f_z_x,
std::bind(&LevMarq::calcDerivates, this, std::placeholders::_1,
std::placeholders::_2, f_z_x));
}
template <typename T>
bool LevMarq<T>::step(F_z_x f_z_x)
{
return step(f_z_x, std::bind(&LevMarq::calcDerivates, this, std::placeholders::_1,
std::placeholders::_2, f_z_x));
}
template <typename T>
void LevMarq<T>::init(eVector &z, F_z_x f_z_x)
{
curr_z = z;
I.resize(z.rows(), z.rows());
I.setIdentity();
f_z_x(curr_z, x64);
minErr = currErr = prevErr = x64.cwiseProduct(x64).sum();
J.resize(x64.rows(), z.rows());
mu = -1;
}
#define splm_get_time(a, b) \
std::chrono::duration_cast<std::chrono::duration<double>>(a - b).count()
template <typename T>
bool LevMarq<T>::step(F_z_x f_z_x, F_z_J f_J)
{
f_J(curr_z, J);
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Jt = J.transpose();
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> JtJ = (Jt * J);
eVector B = -Jt * x64;
if (mu < 0)
{ // first time only
int max = 0;
for (int j = 1; j < JtJ.cols(); j++)
if (JtJ(j, j) > JtJ(max, max))
max = j;
mu = JtJ(max, max) * _tau;
}
double gain = 0, prev_mu = 0;
int ntries = 0;
bool isStepAccepted = false;
do
{
// add/update dumping factor to JtJ.
// very efficient in any case, but particularly if initial dump does not produce improvement and must reenter
for (int j = 0; j < JtJ.cols(); j++)
JtJ(j, j) += mu - prev_mu; // update mu
prev_mu = mu;
eVector delta = JtJ.ldlt().solve(B);
eVector estimated_z = curr_z + delta;
// compute error
f_z_x(estimated_z, x64);
auto err = x64.cwiseProduct(x64).sum();
auto L = 0.5 * delta.transpose() * ((mu * delta) - B);
gain = (err - prevErr) / L(0, 0);
// get gain
if (gain > 0)
{
mu = mu * std::max(double(0.33), 1. - pow(2 * gain - 1, 3));
v = 5.f;
currErr = err;
curr_z = estimated_z;
isStepAccepted = true;
}
else
{
mu = mu * v;
v = v * 5;
}
} while (gain <= 0 && ntries++ < 5);
if (_verbose)
std::cout << std::setprecision(5) << "Curr Error=" << currErr
<< " AErr(prev-curr)=" << prevErr - currErr << " gain=" << gain
<< " dumping factor=" << mu << std::endl;
// //check if we must move to the new position or exit
if (currErr < prevErr)
std::swap(currErr, prevErr);
return isStepAccepted;
}
template <typename T>
double LevMarq<T>::getCurrentSolution(eVector &z)
{
z = curr_z;
return currErr;
}
template <typename T>
double LevMarq<T>::solve(eVector &z, F_z_x f_z_x, F_z_J f_J)
{
init(z, f_z_x);
if (_stopFunction)
{
do
{
step(f_z_x, f_J);
if (_step_callback)
_step_callback(curr_z);
} while (!_stopFunction(curr_z));
}
else
{
// intial error estimation
int mustExit = 0;
for (int i = 0; i < _maxIters && !mustExit; i++)
{
if (_verbose)
std::cerr << "iteration " << i << "/" << _maxIters << " ";
bool isStepAccepted = step(f_z_x, f_J);
// check if we must exit
if (currErr < _minErrorAllowed)
mustExit = 1;
if (fabs(prevErr - currErr) <= _min_step_error_diff || !isStepAccepted)
mustExit = 2;
// exit if error increment
if (currErr < prevErr)
mustExit = 3;
// if ( (prevErr-currErr) < 1e-5 ) mustExit=true;
if (_step_callback)
_step_callback(curr_z);
}
// std::cout<<"Exit code="<<mustExit<<std::endl;
}
z = curr_z;
return currErr;
}
} // namespace aruco
#endif