256 lines
9.1 KiB
C++
256 lines
9.1 KiB
C++
/**
|
|
* @file NonlinearFactorGraph.cpp
|
|
* @brief Factor Graph Constsiting of non-linear factors
|
|
* @brief nonlinearFactorGraph
|
|
* @author Frank Dellaert
|
|
* @author Carlos Nieto
|
|
* @author Christian Potthast
|
|
*/
|
|
|
|
#include <math.h>
|
|
#include <climits>
|
|
#include <stdexcept>
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include "NonlinearFactorGraph.h"
|
|
|
|
using namespace std;
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
LinearFactorGraph NonlinearFactorGraph::linearize(const FGConfig& config) const
|
|
{
|
|
// TODO speed up the function either by returning a pointer or by
|
|
// returning the linearisation as a second argument and returning
|
|
// the reference
|
|
|
|
// create an empty linear FG
|
|
LinearFactorGraph linearFG;
|
|
|
|
// linearize all factors
|
|
for(const_iterator factor=factors.begin(); factor<factors.end(); factor++){
|
|
LinearFactor::shared_ptr lf = (*factor)->linearize(config);
|
|
linearFG.push_back(lf);
|
|
}
|
|
|
|
return linearFG;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
double calculate_error (const NonlinearFactorGraph& fg, const FGConfig& config, int verbosity) {
|
|
double newError = fg.error(config);
|
|
if (verbosity>=1) cout << "error: " << newError << endl;
|
|
return newError;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool check_convergence (double relativeErrorTreshold,
|
|
double absoluteErrorTreshold,
|
|
double currentError, double newError,
|
|
int verbosity)
|
|
{
|
|
// check if diverges
|
|
double absoluteDecrease = currentError - newError;
|
|
if (verbosity>=2) cout << "absoluteDecrease: " << absoluteDecrease << endl;
|
|
if (absoluteDecrease<0)
|
|
throw overflow_error("NonlinearFactorGraph::optimize: error increased, diverges.");
|
|
|
|
// calculate relative error decrease and update currentError
|
|
double relativeDecrease = absoluteDecrease/currentError;
|
|
if (verbosity>=2) cout << "relativeDecrease: " << relativeDecrease << endl;
|
|
bool converged =
|
|
(relativeDecrease < relativeErrorTreshold) ||
|
|
(absoluteDecrease < absoluteErrorTreshold);
|
|
if (verbosity>=1 && converged) cout << "converged" << endl;
|
|
return converged;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// linearize and solve, return delta config (TODO: return updated)
|
|
/* ************************************************************************* */
|
|
FGConfig NonlinearFactorGraph::iterate
|
|
(const FGConfig& config, const Ordering& ordering) const
|
|
{
|
|
LinearFactorGraph linear = linearize(config); // linearize all factors
|
|
return linear.optimize(ordering);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// One iteration of optimize, imperative :-(
|
|
/* ************************************************************************* */
|
|
double NonlinearFactorGraph::iterate
|
|
(FGConfig& config, const Ordering& ordering, int verbosity) const
|
|
{
|
|
FGConfig delta = iterate(config, ordering); // linearize and solve
|
|
if (verbosity>=4) delta.print("delta"); // maybe show output
|
|
config += delta; // update config
|
|
if (verbosity>=3) config.print("config"); // maybe show output
|
|
return calculate_error(*this,config,verbosity);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Ordering NonlinearFactorGraph::getOrdering(FGConfig& config) const
|
|
{
|
|
// TODO: FD: Whoa! This is crazy !!!!! re-linearizing just to get ordering ?
|
|
LinearFactorGraph lfg = linearize(config);
|
|
return lfg.getOrdering();
|
|
}
|
|
/* ************************************************************************* */
|
|
void NonlinearFactorGraph::optimize(FGConfig& config,
|
|
const Ordering& ordering,
|
|
double relativeErrorTreshold,
|
|
double absoluteErrorTreshold,
|
|
int verbosity) const
|
|
{
|
|
bool converged = false;
|
|
double currentError = calculate_error(*this,config, verbosity);
|
|
|
|
// while not converged
|
|
while(!converged) {
|
|
double newError = iterate(config, ordering, verbosity); // linearize, solve, update
|
|
converged = gtsam::check_convergence(relativeErrorTreshold, absoluteErrorTreshold,
|
|
currentError, newError, verbosity);
|
|
currentError = newError;
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool NonlinearFactorGraph::check_convergence(const FGConfig& config1,
|
|
const FGConfig& config2,
|
|
double relativeErrorTreshold,
|
|
double absoluteErrorTreshold,
|
|
int verbosity) const
|
|
{
|
|
double currentError = calculate_error(*this, config1, verbosity);
|
|
double newError = calculate_error(*this, config2, verbosity);
|
|
return gtsam::check_convergence(relativeErrorTreshold, absoluteErrorTreshold,
|
|
currentError, newError, verbosity);
|
|
|
|
}
|
|
|
|
|
|
/* ************************************************************************* */
|
|
// One attempt at a tempered Gauss-Newton step with given lambda
|
|
/* ************************************************************************* */
|
|
pair<FGConfig,double> NonlinearFactorGraph::try_lambda
|
|
(const FGConfig& config, const LinearFactorGraph& linear,
|
|
double lambda, const Ordering& ordering, int verbosity) const
|
|
{
|
|
if (verbosity>=1)
|
|
cout << "trying lambda = " << lambda << endl;
|
|
LinearFactorGraph damped =
|
|
linear.add_priors(sqrt(lambda)); // add prior-factors
|
|
if (verbosity>=7) damped.print("damped");
|
|
FGConfig delta = damped.optimize(ordering); // solve
|
|
if (verbosity>=5) delta.print("delta");
|
|
FGConfig newConfig = config + delta; // update config
|
|
if (verbosity>=5) newConfig.print("config");
|
|
double newError = // calculate...
|
|
calculate_error(*this,newConfig,verbosity>=4); // ...new error
|
|
return make_pair(newConfig,newError); // return config and error
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// One iteration of Levenberg Marquardt, imperative :-(
|
|
/* ************************************************************************* */
|
|
double NonlinearFactorGraph::iterateLM
|
|
(FGConfig& config, double currentError, double& lambda, double lambdaFactor,
|
|
const Ordering& ordering, int verbosity) const
|
|
{
|
|
FGConfig newConfig;
|
|
double newError;
|
|
LinearFactorGraph linear = linearize(config); // linearize all factors once
|
|
if (verbosity>=6) linear.print("linear");
|
|
|
|
// try lambda steps with successively larger lambda until we achieve descent
|
|
boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
|
|
while (newError > currentError) {
|
|
lambda = lambda * lambdaFactor; // be more cautious
|
|
boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
|
|
}
|
|
|
|
// next time, let's be more adventerous
|
|
lambda = lambda / lambdaFactor;
|
|
|
|
// return result of this iteration
|
|
config = newConfig;
|
|
if (verbosity>=4) config.print("config"); // maybe show output
|
|
if (verbosity>=1) cout << "error: " << newError << endl;
|
|
return newError;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void NonlinearFactorGraph::optimizeLM(FGConfig& config,
|
|
const Ordering& ordering,
|
|
double relativeErrorTreshold,
|
|
double absoluteErrorTreshold,
|
|
int verbosity,
|
|
double lambda0,
|
|
double lambdaFactor) const
|
|
{
|
|
double lambda = lambda0;
|
|
bool converged = false;
|
|
double currentError = calculate_error(*this,config, verbosity);
|
|
|
|
if (verbosity>=4) config.print("config"); // maybe show output
|
|
|
|
// while not converged
|
|
while(!converged) {
|
|
// linearize, solve, update
|
|
double newError = iterateLM(config, currentError, lambda, lambdaFactor, ordering, verbosity);
|
|
converged = gtsam::check_convergence(relativeErrorTreshold, absoluteErrorTreshold,
|
|
currentError, newError, verbosity);
|
|
currentError = newError;
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
pair<LinearFactorGraph, FGConfig> NonlinearFactorGraph::OneIterationLM( FGConfig& config,
|
|
const Ordering& ordering,
|
|
double relativeErrorTreshold,
|
|
double absoluteErrorTreshold,
|
|
int verbosity,
|
|
double lambda0,
|
|
double lambdaFactor) const {
|
|
|
|
double lambda = lambda0;
|
|
bool converged = false;
|
|
double currentError = calculate_error(*this,config, verbosity);
|
|
|
|
if (verbosity>=4) config.print("config"); // maybe show output
|
|
|
|
FGConfig newConfig;
|
|
double newError;
|
|
LinearFactorGraph linear = linearize(config); // linearize all factors once
|
|
if (verbosity>=6) linear.print("linear");
|
|
|
|
// try lambda steps with successively larger lambda until we achieve descent
|
|
boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
|
|
while (newError > currentError) {
|
|
lambda = lambda * lambdaFactor; // be more cautious
|
|
boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
|
|
}
|
|
|
|
// next time, let's be more adventerous
|
|
lambda = lambda / lambdaFactor;
|
|
|
|
// return result of this iteration
|
|
config = newConfig;
|
|
if (verbosity>=4) config.print("config"); // maybe show output
|
|
if (verbosity>=1) cout << "error: " << newError << endl;
|
|
|
|
pair<LinearFactorGraph, FGConfig> p(linear, newConfig);
|
|
return p;
|
|
|
|
|
|
|
|
// linearize, solve, update
|
|
//double newError = iterateLM(config, currentError, lambda, lambdaFactor, ordering, verbosity);
|
|
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
|
|
} // namespace gtsam
|