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

Made CG state a class

release/4.3a0
Frank Dellaert 2010-02-14 05:52:20 +00:00
parent 43f9baf77a
commit 1f165a9f85
3 changed files with 88 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
cpp/iterative-inl.h
View File

@ -15,40 +15,61 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
/**
* conjugate gradient method.
* S: linear system, V: step vector, E: errors
*/
// state for CG method
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, double epsilon_abs,
size_t maxIterations, bool steepest = false) {
if (maxIterations == 0) maxIterations = dim(x) * (steepest ? 10 : 1);
size_t reset = (size_t)(sqrt(dim(x))+0.5); // when to reset
struct CGState {
bool steepest, verbose;
double gamma, threshold;
size_t k, maxIterations, reset;
V g, d;
E Ad;
/** constructor */
CGState(const S& Ab, const V& x, bool verb, double epsilon,
double epsilon_abs, size_t maxIt, bool steep) {
k = 0;
verbose = verb;
steepest = steep;
maxIterations == maxIt ? maxIt : dim(x) * (steepest ? 10 : 1);
reset = (size_t) (sqrt(dim(x)) + 0.5); // when to reset
// Start with g0 = A'*(A*x0-b), d0 = - g0
// i.e., first step is in direction of negative gradient
V g = Ab.gradient(x);
V d = g; // instead of negating gradient, alpha will be negated
double gamma0 = dot(g, g), gamma_old = gamma0;
if (gamma0 < epsilon_abs) return x;
double threshold = epsilon * epsilon * gamma0;
g = Ab.gradient(x);
d = g; // instead of negating gradient, alpha will be negated
if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
<< maxIterations << ", ||g0||^2 = " << gamma0 << ", threshold = "
<< threshold << endl;
// init gamma and calculate threshold
gamma = dot(g, g);
threshold = ::max(epsilon_abs, epsilon * epsilon * gamma);
// Allocate and calculate A*d for first iteration
E Ad = Ab * d;
if (gamma > epsilon) Ad = Ab * d;
}
// loop maxIterations times
for (size_t k = 1;; k++) {
/** print */
void print() {
cout << "iteration = " << k << endl;
cout << "dotg = " << gamma << endl;
gtsam::print(g,"g");
gtsam::print(d,"d");
gtsam::print(Ad,"Ad");
}
// calculate optimal step-size
double alpha = - dot(d, g) / dot(Ad, Ad);
/** step the solution */
double takeOptimalStep(V& x) {
double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size
axpy(alpha, d, x); // // do step in new search direction, x += alpha*d
return alpha;
}
// do step in new search direction
axpy(alpha, d, x); // x += alpha*d
if (k==maxIterations) break;
/** take a step, return true if converged */
bool step(const S& Ab, V& x) {
k += 1; // increase iteration number
double alpha = takeOptimalStep(x);
if (k == maxIterations) return true; //---------------------------------->
// update gradient (or re-calculate at reset time)
if (k % reset == 0)
@ -58,17 +79,17 @@ namespace gtsam {
Ab.transposeMultiplyAdd(alpha, Ad, g);
// check for convergence
double gamma = dot(g, g);
if (verbose) cout << "iteration " << k << ": alpha = " << alpha
<< ", dotg = " << gamma << endl;
if (gamma < threshold) break;
double new_gamma = dot(g, g);
if (verbose) cout << "iteration " << k << ": alpha = " << alpha << ", dotg = " << new_gamma << endl;
// print();
if (new_gamma < threshold) return true; //---------------------------------->
// calculate new search direction
if (steepest)
d = g;
else {
double beta = gamma / gamma_old;
gamma_old = gamma;
double beta = new_gamma / gamma;
gamma = new_gamma;
// d = g + d*beta;
scal(beta, d);
axpy(1.0, g, d);
@ -76,7 +97,29 @@ namespace gtsam {
// In-place recalculation Ad <- A*d to avoid re-allocating Ad
Ab.multiplyInPlace(d, Ad);
return false;
}
};
/**
* conjugate gradient method.
* S: linear system, V: step vector, E: errors
*/
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon,
double epsilon_abs, size_t maxIterations, bool steepest = false) {
CGState<S, V, E> state(Ab, x, verbose, epsilon, epsilon_abs, maxIterations, steepest);
if (state.gamma < state.threshold) return x;
if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
<< maxIterations << ", ||g0||^2 = " << state.gamma
<< ", threshold = " << state.threshold << endl;
// loop maxIterations times
while (!state.step(Ab, x))
;
return x;
}

15
cpp/testIterative.cpp
View File

@ -26,6 +26,8 @@ using namespace std;
using namespace gtsam;
using namespace example;
static bool verbose = false;
/* ************************************************************************* */
TEST( Iterative, steepestDescent )
{
@ -38,7 +40,6 @@ TEST( Iterative, steepestDescent )
// Do gradient descent
GaussianFactorGraph fg2 = createGaussianFactorGraph();
VectorConfig zero = createZeroDelta();
bool verbose = false;
VectorConfig actual = steepestDescent(fg2, zero, verbose);
CHECK(assert_equal(expected,actual,1e-2));
}
@ -62,20 +63,20 @@ TEST( Iterative, conjugateGradientDescent )
// Do conjugate gradient descent, System version
System Ab(A, b);
Vector actualX = conjugateGradientDescent(Ab, x0);
Vector actualX = conjugateGradientDescent(Ab, x0, verbose);
CHECK(assert_equal(expectedX,actualX,1e-9));
// Do conjugate gradient descent, Matrix version
Vector actualX2 = conjugateGradientDescent(A, b, x0);
Vector actualX2 = conjugateGradientDescent(A, b, x0, verbose);
CHECK(assert_equal(expectedX,actualX2,1e-9));
// Do conjugate gradient descent on factor graph
VectorConfig zero = createZeroDelta();
VectorConfig actual = conjugateGradientDescent(fg2, zero);
VectorConfig actual = conjugateGradientDescent(fg2, zero, verbose);
CHECK(assert_equal(expected,actual,1e-2));
// Test method
VectorConfig actual2 = fg2.conjugateGradientDescent(zero);
VectorConfig actual2 = fg2.conjugateGradientDescent(zero, verbose);
CHECK(assert_equal(expected,actual2,1e-2));
}
@ -122,7 +123,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
zeros.insert("x2",zero(3));
GaussianFactorGraph fg = graph.linearize(config);
VectorConfig actual = conjugateGradientDescent(fg, zeros, false, 1e-3, 1e-5, 100);
VectorConfig actual = conjugateGradientDescent(fg, zeros, verbose, 1e-3, 1e-5, 100);
VectorConfig expected;
expected.insert("x1", zero(3));
@ -169,7 +170,7 @@ TEST( Iterative, subgraphPCG )
// Solve the subgraph PCG
VectorConfig ybar = conjugateGradients<SubgraphPreconditioner, VectorConfig,
Errors> (system, zeros, false, 1e-5, 1e-5, 100);
Errors> (system, zeros, verbose, 1e-5, 1e-5, 100);
VectorConfig actual = system.x(ybar);
VectorConfig expected;

2
cpp/testSubgraphPreconditioner.cpp
View File

@ -210,7 +210,7 @@ TEST( SubgraphPreconditioner, conjugateGradients )
// Compare with non preconditioned version:
VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon,
maxIterations);
CHECK(assert_equal(xtrue,actual2,1e-5));
CHECK(assert_equal(xtrue,actual2,1e-4));
}
/* ************************************************************************* */