90 lines
3.1 KiB
C++
90 lines
3.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;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Ordering NonlinearFactorGraph::getOrdering(const FGConfig& config) const {
|
|
// TODO: FD: Whoa! This is crazy !!!!! re-linearizing just to get ordering ?
|
|
LinearFactorGraph lfg = linearize(config);
|
|
return lfg.getOrdering();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // namespace gtsam
|