110 lines
2.8 KiB
C++
110 lines
2.8 KiB
C++
/*
|
|
* iterative.cpp
|
|
* @brief Iterative methods, implementation
|
|
* @author Frank Dellaert
|
|
* Created on: Dec 28, 2009
|
|
*/
|
|
|
|
#include "GaussianFactorGraph.h"
|
|
#include "iterative.h"
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
|
|
/**
|
|
* gradient of objective function 0.5*|Ax-b|^2 at x = A'*(Ax-b)
|
|
*/
|
|
Vector gradient(const System& Ab, const Vector& x) {
|
|
const Matrix& A = Ab.first;
|
|
const Vector& b = Ab.second;
|
|
return A ^ (A * x - b);
|
|
}
|
|
|
|
/**
|
|
* Apply operator A
|
|
*/
|
|
Vector operator*(const System& Ab, const Vector& x) {
|
|
const Matrix& A = Ab.first;
|
|
return A * x;
|
|
}
|
|
|
|
/**
|
|
* Apply operator A^T
|
|
*/
|
|
Vector operator^(const System& Ab, const Vector& x) {
|
|
const Matrix& A = Ab.first;
|
|
return A ^ x;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Method of conjugate gradients (CG) template
|
|
// "System" class S needs gradient(S,v), e=S*v, v=S^e
|
|
// "Vector" class V needs dot(v,v), -v, v+v, s*v
|
|
// "Vector" class E needs dot(v,v)
|
|
template<class S, class V, class E>
|
|
V CGD(const S& Ab, V x, double threshold = 1e-9) {
|
|
|
|
// Start with g0 = A'*(A*x0-b), d0 = - g0
|
|
// i.e., first step is in direction of negative gradient
|
|
V g = gradient(Ab, x);
|
|
V d = -g;
|
|
double prev_dotg = dot(g, g);
|
|
|
|
// loop max n times
|
|
size_t n = x.size();
|
|
for (int k = 1; k <= n; k++) {
|
|
|
|
// calculate optimal step-size
|
|
E Ad = Ab * d;
|
|
double alpha = -dot(d, g) / dot(Ad, Ad);
|
|
|
|
// do step in new search direction
|
|
x = x + alpha * d;
|
|
if (k == n) break;
|
|
|
|
// update gradient
|
|
g = g + alpha * (Ab ^ Ad);
|
|
|
|
// check for convergence
|
|
double dotg = dot(g, g);
|
|
if (dotg < threshold) break;
|
|
|
|
// calculate new search direction
|
|
double beta = dotg / prev_dotg;
|
|
prev_dotg = dotg;
|
|
d = -g + beta * d;
|
|
}
|
|
return x;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector conjugateGradientDescent(const System& Ab, const Vector& x,
|
|
double threshold) {
|
|
return CGD<System, Vector, Vector> (Ab, x);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
|
|
const Vector& x, double threshold) {
|
|
System Ab = make_pair(A, b);
|
|
return CGD<System, Vector, Vector> (Ab, x);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
VectorConfig gradient(const GaussianFactorGraph& fg, const VectorConfig& x) {
|
|
return fg.gradient(x);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg,
|
|
const VectorConfig& x, double threshold) {
|
|
return CGD<GaussianFactorGraph, VectorConfig, Errors> (fg, x);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // namespace gtsam
|