diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index bcd4e52d9..0aec3e9f2 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -32,17 +32,6 @@ namespace gtsam { struct SharedDiagonal; - /** unnormalized error - * \todo Make this a member function - affects SubgraphPreconditioner */ - template - double gaussianError(const FactorGraph& fg, const VectorValues& x) { - double total_error = 0.; - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) { - total_error += factor->error(x); - } - return total_error; - } - /** return A*x-b * \todo Make this a member function - affects SubgraphPreconditioner */ template @@ -131,9 +120,17 @@ namespace gtsam { /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); + /** unnormalized error */ + double error(const VectorValues& x) const { + double total_error = 0.; + BOOST_FOREACH(const sharedFactor& factor, *this) + total_error += factor->error(x); + return total_error; + } + /** Unnormalized probability. O(n) */ double probPrime(const VectorValues& c) const { - return exp(-0.5 * gaussianError(*this, c)); + return exp(-0.5 * error(c)); } /** diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index 4e25d954d..ca7f805ac 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -54,17 +54,17 @@ TEST( GaussianFactorGraph, equals ) { } /* ************************************************************************* */ -TEST( GaussianFactorGraph, error ) { - Ordering ordering; ordering += "x1","x2","l1"; - FactorGraph fg = createGaussianFactorGraph(ordering); - VectorValues cfg = createZeroDelta(ordering); - - // note the error is the same as in testNonlinearFactorGraph as a - // zero delta config in the linear graph is equivalent to noisy in - // non-linear, which is really linear under the hood - double actual = gaussianError(fg, cfg); - DOUBLES_EQUAL( 5.625, actual, 1e-9 ); -} +//TEST( GaussianFactorGraph, error ) { +// Ordering ordering; ordering += "x1","x2","l1"; +// FactorGraph fg = createGaussianFactorGraph(ordering); +// VectorValues cfg = createZeroDelta(ordering); +// +// // note the error is the same as in testNonlinearFactorGraph as a +// // zero delta config in the linear graph is equivalent to noisy in +// // non-linear, which is really linear under the hood +// double actual = fg.error(cfg); +// DOUBLES_EQUAL( 5.625, actual, 1e-9 ); +//} /* ************************************************************************* */ /* unit test for find seperator */ @@ -666,7 +666,7 @@ double error(const VectorValues& x) { Ordering ord; ord += "x2","l1","x1"; GaussianFactorGraph fg = createGaussianFactorGraph(ord); - return gaussianError(fg,x); + return fg.error(x); } ///* ************************************************************************* */