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, VectorConfig GaussianFactorGraph::steepestDescent(const VectorConfig& x0,
double epsilon, size_t maxIterations) const { bool verbose, double epsilon, size_t maxIterations) const {
return gtsam::steepestDescent(*this, x0, epsilon, maxIterations); return gtsam::steepestDescent(*this, x0, verbose, epsilon, maxIterations);
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<VectorConfig> GaussianFactorGraph::steepestDescent_( 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( return boost::shared_ptr<VectorConfig>(new VectorConfig(
gtsam::conjugateGradientDescent(*this, x0, epsilon, maxIterations))); gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
maxIterations)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig GaussianFactorGraph::conjugateGradientDescent( VectorConfig GaussianFactorGraph::conjugateGradientDescent(
const VectorConfig& x0, double epsilon, size_t maxIterations) const { const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const {
return gtsam::conjugateGradientDescent(*this, x0, epsilon, maxIterations); return gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
maxIterations);
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<VectorConfig> GaussianFactorGraph::conjugateGradientDescent_( 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( 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 * @param x0: VectorConfig specifying initial estimate
* @return solution * @return solution
*/ */
VectorConfig steepestDescent(const VectorConfig& x0, double epsilon = 1e-5, VectorConfig steepestDescent(const VectorConfig& x0, bool verbose = false,
size_t maxIterations = 0) const; double epsilon = 1e-3, size_t maxIterations = 0) const;
/** /**
* shared pointer versions for MATLAB * shared pointer versions for MATLAB
*/ */
boost::shared_ptr<VectorConfig> boost::shared_ptr<VectorConfig>
steepestDescent_(const VectorConfig& x0, double epsilon, steepestDescent_(const VectorConfig& x0, bool verbose = false,
size_t maxIterations) const; double epsilon = 1e-3, size_t maxIterations = 0) const;
/** /**
* Find solution using conjugate gradient descent * Find solution using conjugate gradient descent
* @param x0: VectorConfig specifying initial estimate * @param x0: VectorConfig specifying initial estimate
* @return solution * @return solution
*/ */
VectorConfig conjugateGradientDescent(const VectorConfig& x0, VectorConfig conjugateGradientDescent(const VectorConfig& x0, bool verbose =
double epsilon = 1e-5, size_t maxIterations = 0) const; false, double epsilon = 1e-3, size_t maxIterations = 0) const;
/** /**
* shared pointer versions for MATLAB * shared pointer versions for MATLAB
*/ */
boost::shared_ptr<VectorConfig> conjugateGradientDescent_( 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) // "Vector" class E needs dot(v,v)
// if (steepest) does steepest descent // if (steepest) does steepest descent
template<class S, class V, class E> template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, size_t maxIterations, double epsilon, V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon,
bool steepest = false) { 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 // Start with g0 = A'*(A*x0-b), d0 = - g0
// i.e., first step is in direction of negative gradient // 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 dotg0 = dot(g, g), prev_dotg = dotg0;
double threshold = epsilon * epsilon * dotg0; double threshold = epsilon * epsilon * dotg0;
// loop max n times if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
size_t n = x.size(); << maxIterations << ", ||g0||^2 = " << dotg0 << ", threshold = "
<< threshold << endl;
// loop maxIterations times
for (size_t k = 0; k < maxIterations; k++) { for (size_t k = 0; k < maxIterations; k++) {
// calculate optimal step-size // calculate optimal step-size
@ -41,13 +44,14 @@ namespace gtsam {
// do step in new search direction // do step in new search direction
x = x + alpha * d; x = x + alpha * d;
if (k == n) break;
// update gradient // update gradient
g = g + alpha * (Ab ^ Ad); g = g + alpha * (Ab ^ Ad);
// check for convergence // check for convergence
double dotg = dot(g, g); double dotg = dot(g, g);
if (verbose) cout << "iteration " << k << ": alpha = " << alpha
<< ", dotg = " << dotg << endl;
if (dotg < threshold) break; if (dotg < threshold) break;
// calculate new search direction // calculate new search direction
@ -83,30 +87,30 @@ namespace gtsam {
return A ^ x; return A ^ x;
} }
Vector steepestDescent(const System& Ab, const Vector& x, double epsilon, Vector steepestDescent(const System& Ab, const Vector& x, bool verbose,
size_t maxIterations) { double epsilon, size_t maxIterations) {
return conjugateGradients<System, Vector, Vector> (Ab, x, epsilon, return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
maxIterations, true); maxIterations, true);
} }
Vector conjugateGradientDescent(const System& Ab, const Vector& x, Vector conjugateGradientDescent(const System& Ab, const Vector& x,
double epsilon, size_t maxIterations) { bool verbose, double epsilon, size_t maxIterations) {
return conjugateGradients<System, Vector, Vector> (Ab, x, epsilon, return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
maxIterations); maxIterations);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, 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); 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); maxIterations, true);
} }
Vector conjugateGradientDescent(const Matrix& A, const Vector& b, 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); System Ab = make_pair(A, b);
return conjugateGradients<System, Vector, Vector> (Ab, x, epsilon, return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
maxIterations); maxIterations);
} }
@ -116,15 +120,15 @@ namespace gtsam {
} }
VectorConfig steepestDescent(const GaussianFactorGraph& fg, 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, return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg,
x, epsilon, maxIterations, true); x, verbose, epsilon, maxIterations, true);
} }
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg, 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, 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 * Method of steepest gradients, System version
*/ */
Vector steepestDescent(const System& Ab, const Vector& x, double epsilon = Vector steepestDescent(const System& Ab, const Vector& x, bool verbose =
1e-5, size_t maxIterations = 0); false, double epsilon = 1e-3, size_t maxIterations = 0);
/** /**
* Method of steepest gradients, Matrix version * Method of steepest gradients, Matrix version
*/ */
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, 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 * Method of steepest gradients, Gaussian Factor Graph version
* */ * */
VectorConfig steepestDescent(const GaussianFactorGraph& fg, 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 * Method of conjugate gradients (CG), System version
*/ */
Vector conjugateGradientDescent(const System& Ab, const Vector& x, 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 * Method of conjugate gradients (CG), Matrix version
*/ */
Vector conjugateGradientDescent(const Matrix& A, const Vector& b, 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 * Method of conjugate gradients (CG), Gaussian Factor Graph version
* */ * */
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg, 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 } // namespace gtsam

8
cpp/testIterative.cpp
View File

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