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

correct convergence criterion, verbose flag

release/4.3a0
Frank Dellaert 2009-12-28 16:15:26 +00:00
parent 674ae9d030
commit 5ac304aff3
5 changed files with 57 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cpp/GaussianFactorGraph.cpp
View File

@ -283,28 +283,31 @@ VectorConfig GaussianFactorGraph::optimalUpdate(const VectorConfig& x,
/* ************************************************************************* */
VectorConfig GaussianFactorGraph::steepestDescent(const VectorConfig& x0,
double epsilon, size_t maxIterations) const {
return gtsam::steepestDescent(*this, x0, epsilon, maxIterations);
bool verbose, double epsilon, size_t maxIterations) const {
return gtsam::steepestDescent(*this, x0, verbose, epsilon, maxIterations);
}
/* ************************************************************************* */
boost::shared_ptr<VectorConfig> GaussianFactorGraph::steepestDescent_(
const VectorConfig& x0, double epsilon, size_t maxIterations) const {
const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const {
return boost::shared_ptr<VectorConfig>(new VectorConfig(
gtsam::conjugateGradientDescent(*this, x0, epsilon, maxIterations)));
gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
maxIterations)));
}
/* ************************************************************************* */
VectorConfig GaussianFactorGraph::conjugateGradientDescent(
const VectorConfig& x0, double epsilon, size_t maxIterations) const {
return gtsam::conjugateGradientDescent(*this, x0, epsilon, maxIterations);
const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const {
return gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
maxIterations);
}
/* ************************************************************************* */
boost::shared_ptr<VectorConfig> GaussianFactorGraph::conjugateGradientDescent_(
const VectorConfig& x0, double epsilon, size_t maxIterations) const {
const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const {
return boost::shared_ptr<VectorConfig>(new VectorConfig(
gtsam::conjugateGradientDescent(*this, x0, epsilon, maxIterations)));
gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
maxIterations)));
}
/* ************************************************************************* */

15
cpp/GaussianFactorGraph.h
View File

@ -189,29 +189,30 @@ namespace gtsam {
* @param x0: VectorConfig specifying initial estimate
* @return solution
*/
VectorConfig steepestDescent(const VectorConfig& x0, double epsilon = 1e-5,
size_t maxIterations = 0) const;
VectorConfig steepestDescent(const VectorConfig& x0, bool verbose = false,
double epsilon = 1e-3, size_t maxIterations = 0) const;
/**
* shared pointer versions for MATLAB
*/
boost::shared_ptr<VectorConfig>
steepestDescent_(const VectorConfig& x0, double epsilon,
size_t maxIterations) const;
steepestDescent_(const VectorConfig& x0, bool verbose = false,
double epsilon = 1e-3, size_t maxIterations = 0) const;
/**
* Find solution using conjugate gradient descent
* @param x0: VectorConfig specifying initial estimate
* @return solution
*/
VectorConfig conjugateGradientDescent(const VectorConfig& x0,
double epsilon = 1e-5, size_t maxIterations = 0) const;
VectorConfig conjugateGradientDescent(const VectorConfig& x0, bool verbose =
false, double epsilon = 1e-3, size_t maxIterations = 0) const;
/**
* shared pointer versions for MATLAB
*/
boost::shared_ptr<VectorConfig> conjugateGradientDescent_(
const VectorConfig& x0, double epsilon, size_t maxIterations) const;
const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3,
size_t maxIterations = 0) const;
};
}

42
cpp/iterative.cpp
View File

@ -19,10 +19,10 @@ namespace gtsam {
// "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, size_t maxIterations, double epsilon,
bool steepest = false) {
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon,
size_t maxIterations, bool steepest = false) {
if (maxIterations == 0) maxIterations = dim(x);
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
@ -31,8 +31,11 @@ namespace gtsam {
double dotg0 = dot(g, g), prev_dotg = dotg0;
double threshold = epsilon * epsilon * dotg0;
// loop max n times
size_t n = x.size();
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
@ -41,13 +44,14 @@ namespace gtsam {
// 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 (verbose) cout << "iteration " << k << ": alpha = " << alpha
<< ", dotg = " << dotg << endl;
if (dotg < threshold) break;
// calculate new search direction
@ -83,30 +87,30 @@ namespace gtsam {
return A ^ x;
}
Vector steepestDescent(const System& Ab, const Vector& x, double epsilon,
size_t maxIterations) {
return conjugateGradients<System, Vector, Vector> (Ab, x, epsilon,
Vector steepestDescent(const System& Ab, const Vector& x, bool verbose,
double epsilon, size_t maxIterations) {
return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
maxIterations, true);
}
Vector conjugateGradientDescent(const System& Ab, const Vector& x,
double epsilon, size_t maxIterations) {
return conjugateGradients<System, Vector, Vector> (Ab, x, epsilon,
bool verbose, double epsilon, size_t maxIterations) {
return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
maxIterations);
}
/* ************************************************************************* */
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
double epsilon, size_t maxIterations) {
bool verbose, double epsilon, size_t maxIterations) {
System Ab = make_pair(A, b);
return conjugateGradients<System, Vector, Vector> (Ab, x, epsilon,
return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
maxIterations, true);
}
Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
const Vector& x, double epsilon, size_t maxIterations) {
const Vector& x, bool verbose, double epsilon, size_t maxIterations) {
System Ab = make_pair(A, b);
return conjugateGradients<System, Vector, Vector> (Ab, x, epsilon,
return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
maxIterations);
}
@ -116,15 +120,15 @@ namespace gtsam {
}
VectorConfig steepestDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, double epsilon, size_t maxIterations) {
const VectorConfig& x, bool verbose, double epsilon, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg,
x, epsilon, maxIterations, true);
x, verbose, epsilon, maxIterations, true);
}
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, double epsilon, size_t maxIterations) {
const VectorConfig& x, bool verbose, double epsilon, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg,
x, epsilon, maxIterations);
x, verbose, epsilon, maxIterations);
}
/* ************************************************************************* */

17
cpp/iterative.h
View File

@ -23,37 +23,40 @@ namespace gtsam {
/**
* Method of steepest gradients, System version
*/
Vector steepestDescent(const System& Ab, const Vector& x, double epsilon =
1e-5, size_t maxIterations = 0);
Vector steepestDescent(const System& Ab, const Vector& x, bool verbose =
false, double epsilon = 1e-3, size_t maxIterations = 0);
/**
* Method of steepest gradients, Matrix version
*/
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
double epsilon = 1e-5, size_t maxIterations = 0);
bool verbose = false, double epsilon = 1e-3, size_t maxIterations = 0);
/**
* Method of steepest gradients, Gaussian Factor Graph version
* */
VectorConfig steepestDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, double epsilon = 1e-5, size_t maxIterations = 0);
const VectorConfig& x, bool verbose = false, double epsilon = 1e-3,
size_t maxIterations = 0);
/**
* Method of conjugate gradients (CG), System version
*/
Vector conjugateGradientDescent(const System& Ab, const Vector& x,
double epsilon = 1e-5, size_t maxIterations = 0);
bool verbose = false, double epsilon = 1e-3, size_t maxIterations = 0);
/**
* Method of conjugate gradients (CG), Matrix version
*/
Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
const Vector& x, double epsilon = 1e-5, size_t maxIterations = 0);
const Vector& x, bool verbose = false, double epsilon = 1e-3,
size_t maxIterations = 0);
/**
* Method of conjugate gradients (CG), Gaussian Factor Graph version
* */
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, double epsilon = 1e-5, size_t maxIterations = 0);
const VectorConfig& x, bool verbose = false, double epsilon = 1e-3,
size_t maxIterations = 0);
} // namespace gtsam

8
cpp/testIterative.cpp
View File

@ -28,8 +28,9 @@ TEST( Iterative, steepestDescent )
// Do gradient descent
GaussianFactorGraph fg2 = createGaussianFactorGraph();
VectorConfig zero = createZeroDelta();
VectorConfig actual = fg2.steepestDescent(zero);
//CHECK(assert_equal(expected,actual,1e-2));
bool verbose = false;
VectorConfig actual = steepestDescent(fg2, zero, verbose);
CHECK(assert_equal(expected,actual,1e-2));
}
/* ************************************************************************* */
@ -43,7 +44,8 @@ TEST( Iterative, conjugateGradientDescent )
// create graph and get matrices
GaussianFactorGraph fg2 = createGaussianFactorGraph();
Matrix A; Vector b;
Matrix A;
Vector b;
Vector x0 = gtsam::zero(6);
boost::tie(A, b) = fg2.matrix(ord);
Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2);