12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

template now exposed in iterative.h, and has its own implementation file

release/4.3a0
Frank Dellaert 2009-12-28 16:26:16 +00:00
parent 5ac304aff3
commit 0c0b73042b
4 changed files with 81 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Makefile.am
View File

@ -100,7 +100,7 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp
testBinaryBayesNet_LDADD = libgtsam.la
# Gaussian inference
headers += GaussianFactorSet.h
headers += GaussianFactorSet.h iterative-inl.h
sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp iterative.cpp
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testIterative
testVectorConfig_SOURCES = testVectorConfig.cpp

66
cpp/iterative-inl.h Normal file
View File

@ -0,0 +1,66 @@
/*
* iterative-inl.h
* @brief Iterative methods, template implementation
* @author Frank Dellaert
* Created on: Dec 28, 2009
*/
#include "GaussianFactorGraph.h"
#include "iterative.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon,
size_t maxIterations, bool steepest = false) {
if (maxIterations == 0) maxIterations = dim(x) * (steepest ? 10 : 1);
// 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 dotg0 = dot(g, g), prev_dotg = dotg0;
double threshold = epsilon * epsilon * dotg0;
if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
<< maxIterations << ", ||g0||^2 = " << dotg0 << ", threshold = "
<< threshold << endl;
// loop maxIterations times
for (size_t k = 0; k < maxIterations; 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;
// update gradient
g = g + alpha * (Ab ^ Ad);
// check for convergence
double dotg = dot(g, g);
if (verbose) cout << "iteration " << k << ": alpha = " << alpha
<< ", dotg = " << dotg << endl;
if (dotg < threshold) break;
// calculate new search direction
if (steepest)
d = -g;
else {
double beta = dotg / prev_dotg;
prev_dotg = dotg;
d = -g + beta * d;
}
}
return x;
}
/* ************************************************************************* */
} // namespace gtsam

56
cpp/iterative.cpp
View File

@ -6,66 +6,12 @@
*/
#include "GaussianFactorGraph.h"
#include "iterative.h"
#include "iterative-inl.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
// 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)
// if (steepest) does steepest descent
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon,
size_t maxIterations, bool steepest = false) {
if (maxIterations == 0) maxIterations = dim(x) * (steepest ? 10 : 1);
// 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 dotg0 = dot(g, g), prev_dotg = dotg0;
double threshold = epsilon * epsilon * dotg0;
if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
<< maxIterations << ", ||g0||^2 = " << dotg0 << ", threshold = "
<< threshold << endl;
// loop maxIterations times
for (size_t k = 0; k < maxIterations; 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;
// update gradient
g = g + alpha * (Ab ^ Ad);
// check for convergence
double dotg = dot(g, g);
if (verbose) cout << "iteration " << k << ": alpha = " << alpha
<< ", dotg = " << dotg << endl;
if (dotg < threshold) break;
// calculate new search direction
if (steepest)
d = -g;
else {
double beta = dotg / prev_dotg;
prev_dotg = dotg;
d = -g + beta * d;
}
}
return x;
}
/* ************************************************************************* */
/** gradient of objective function 0.5*|Ax-b|^2 at x = A'*(Ax-b) */

17
cpp/iterative.h
View File

@ -15,10 +15,19 @@ namespace gtsam {
typedef std::pair<Matrix, Vector> System;
/**
* In all calls below
* x is the initial estimate
* epsilon determines the convergence criterion: norm(g)<epsilon*norm(g0)
*/
* 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)
* @param Ab, the "system" that needs to be solved, examples below
* @param x is the initial estimate
* @param epsilon determines the convergence criterion: norm(g)<epsilon*norm(g0)
* @param maxIterations, if 0 will be set to |x|
* @param steepest flag, if true does steepest descent, not CG
* */
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon,
size_t maxIterations, bool steepest = false);
/**
* Method of steepest gradients, System version