diff --git a/cpp/BayesNetPreconditioner.cpp b/cpp/BayesNetPreconditioner.cpp new file mode 100644 index 000000000..2fb4ab44f --- /dev/null +++ b/cpp/BayesNetPreconditioner.cpp @@ -0,0 +1,60 @@ +/* + * BayesNetPreconditioner.cpp + * Created on: Dec 31, 2009 + * @Author: Frank Dellaert + */ + +#include +#include "BayesNetPreconditioner.h" + +namespace gtsam { + + /* ************************************************************************* */ + BayesNetPreconditioner::BayesNetPreconditioner(const GaussianFactorGraph& Ab, + const GaussianBayesNet& Rd) : + Ab_(Ab), Rd_(Rd) { + } + + /* ************************************************************************* */ + // R*x = y by solving x=inv(R)*y + VectorConfig BayesNetPreconditioner::backSubstitute(const VectorConfig& y) const { + return gtsam::backSubstitute(Rd_, y); + } + + /* ************************************************************************* */ + // gy=inv(L)*gx by solving L*gy=gx. + VectorConfig BayesNetPreconditioner::backSubstituteTranspose( + const VectorConfig& gx) const { + return gtsam::backSubstituteTranspose(Rd_, gx); + } + + /* ************************************************************************* */ + double BayesNetPreconditioner::error(const VectorConfig& y) const { + return Ab_.error(x(y)); + } + + /* ************************************************************************* */ + // gradient is inv(R')*A'*(A*inv(R)*y-b), + VectorConfig BayesNetPreconditioner::gradient(const VectorConfig& y) const { + VectorConfig gx = Ab_ ^ Ab_.errors(x(y)); + return gtsam::backSubstituteTranspose(Rd_, gx); + } + + /* ************************************************************************* */ + // Apply operator * + Errors BayesNetPreconditioner::operator*(const VectorConfig& y) const { + return Ab_ * x(y); + } + + /* ************************************************************************* */ + // Apply operator inv(R')*A'*e + VectorConfig BayesNetPreconditioner::operator^(const Errors& e) const { + VectorConfig x = Ab_ ^ e; // x = A'*e2 + return gtsam::backSubstituteTranspose(Rd_, x); + } + + /* ************************************************************************* */ + +} // namespace gtsam + + diff --git a/cpp/BayesNetPreconditioner.h b/cpp/BayesNetPreconditioner.h new file mode 100644 index 000000000..176118633 --- /dev/null +++ b/cpp/BayesNetPreconditioner.h @@ -0,0 +1,62 @@ +/* + * BayesNetPreconditioner.h + * Created on: Dec 31, 2009 + * @Author: Frank Dellaert + */ + +#ifndef BAYESNETPRECONDITIONER_H_ +#define BAYESNETPRECONDITIONER_H_ + +#include "GaussianFactorGraph.h" +#include "GaussianBayesNet.h" + +namespace gtsam { + + /** + * Upper-triangular preconditioner R for the system |A*x-b|^2 + * The new system will be |A*inv(R)*y-b|^2, i.e., R*x=y + * This class can solve for x=inv(R)*y by back-substituting R*x=y + * and also apply the chain rule gy=inv(R')*gx by solving R'*gy=gx. + * This is not used currently, just to debug operators below + */ + class BayesNetPreconditioner { + + // The original system + const GaussianFactorGraph& Ab_; + + // The preconditioner + const GaussianBayesNet& Rd_; + + public: + + /** Constructor */ + BayesNetPreconditioner(const GaussianFactorGraph& Ab, + const GaussianBayesNet& Rd); + + // R*x = y by solving x=inv(R)*y + VectorConfig backSubstitute(const VectorConfig& y) const; + + // gy=inv(L)*gx by solving L*gy=gx. + VectorConfig backSubstituteTranspose(const VectorConfig& gx) const; + + /* x = inv(R)*y */ + inline VectorConfig x(const VectorConfig& y) const { + return backSubstitute(y); + } + + /* error, given y */ + double error(const VectorConfig& y) const; + + /** gradient */ + VectorConfig gradient(const VectorConfig& y) const; + + /** Apply operator A */ + Errors operator*(const VectorConfig& y) const; + + /** Apply operator A' */ + VectorConfig operator^(const Errors& e) const; + }; + +} // namespace gtsam + +#endif /* BAYESNETPRECONDITIONER_H_ */ diff --git a/cpp/Makefile.am b/cpp/Makefile.am index e5197dd1c..f48d2acab 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -96,9 +96,9 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp testBinaryBayesNet_LDADD = libgtsam.la # Gaussian inference -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 +headers += GaussianFactorSet.h +sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp +check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testVectorConfig_SOURCES = testVectorConfig.cpp testVectorConfig_LDADD = libgtsam.la testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp @@ -109,8 +109,17 @@ testGaussianConditional_SOURCES = $(example) testGaussianConditional.cpp testGaussianConditional_LDADD = libgtsam.la testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp testGaussianBayesNet_LDADD = libgtsam.la -testIterative_SOURCES = $(example) testIterative.cpp -testIterative_LDADD = libgtsam.la + +# Iterative Methods +headers += iterative-inl.h +sources += iterative.cpp BayesNetPreconditioner.cpp subgraphPreconditioner.cpp +check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner +testIterative_SOURCES = $(example) testIterative.cpp +testIterative_LDADD = libgtsam.la +testBayesNetPreconditioner_SOURCES = $(example) testBayesNetPreconditioner.cpp +testBayesNetPreconditioner_LDADD = libgtsam.la +testSubgraphPreconditioner_SOURCES = $(example) testSubgraphPreconditioner.cpp +testSubgraphPreconditioner_LDADD = libgtsam.la # not the correct way, I'm sure: Kai ? timeGaussianFactor: timeGaussianFactor.cpp diff --git a/cpp/SubgraphPreconditioner.cpp b/cpp/SubgraphPreconditioner.cpp new file mode 100644 index 000000000..29f35f063 --- /dev/null +++ b/cpp/SubgraphPreconditioner.cpp @@ -0,0 +1,101 @@ +/* + * SubgraphPreconditioner.cpp + * Created on: Dec 31, 2009 + * @author: Frank Dellaert + */ + +#include +#include "SubgraphPreconditioner.h" + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + SubgraphPreconditioner::SubgraphPreconditioner(const GaussianBayesNet& Rc1, + const GaussianFactorGraph& Ab2, const VectorConfig& xbar) : + Rc1_(Rc1), Ab2_(Ab2), xbar_(xbar), b2bar_(Ab2_.errors(xbar)) { + } + + /* ************************************************************************* */ + // x = xbar + inv(R1)*y + VectorConfig SubgraphPreconditioner::x(const VectorConfig& y) const { + return xbar_ + gtsam::backSubstitute(Rc1_, y); + } + + /* ************************************************************************* */ + double SubgraphPreconditioner::error(const VectorConfig& y) const { + + Errors e; + + // Use BayesNet order to add y contributions in order + BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) { + const string& j = cg->key(); + e.push_back(y[j]); // append y + } + + // Add A2 contribution + VectorConfig x = this->x(y); + Errors e2 = Ab2_.errors(x); + e.splice(e.end(), e2); + + return 0.5 * dot(e, e); + } + + /* ************************************************************************* */ + // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), + VectorConfig SubgraphPreconditioner::gradient(const VectorConfig& y) const { + VectorConfig x = this->x(y); // x = inv(R1)*y + VectorConfig gx2 = Ab2_ ^ Ab2_.errors(x); + VectorConfig gy2 = gtsam::backSubstituteTranspose(Rc1_, gx2); // inv(R1')*gx2 + return y + gy2; + } + + /* ************************************************************************* */ + // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] + Errors SubgraphPreconditioner::operator*(const VectorConfig& y) const { + + Errors e; + + // Use BayesNet order to add y contributions in order + BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) { + const string& j = cg->key(); + e.push_back(y[j]); // append y + } + + // Add A2 contribution + VectorConfig x = gtsam::backSubstitute(Rc1_, y); // x=inv(R1)*y + Errors e2 = Ab2_ * x; // A2*x + e.splice(e.end(), e2); + + return e; + } + + /* ************************************************************************* */ + // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 + VectorConfig SubgraphPreconditioner::operator^(const Errors& e) const { + + VectorConfig y1; + + // Use BayesNet order to remove y contributions in order + Errors::const_iterator it = e.begin(); + BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) { + const string& j = cg->key(); + const Vector& ej = *(it++); + y1.insert(j,ej); + } + + // create e2 with what's left of e + Errors e2; + while (it != e.end()) + e2.push_back(*(it++)); + + // get A2 part, + VectorConfig x = Ab2_ ^ e2; // x = A2'*e2 + VectorConfig y2 = gtsam::backSubstituteTranspose(Rc1_, x); // inv(R1')*x; + + return y1 + y2; + } + /* ************************************************************************* */ + +} // nsamespace gtsam diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h new file mode 100644 index 000000000..3e92b3d67 --- /dev/null +++ b/cpp/SubgraphPreconditioner.h @@ -0,0 +1,61 @@ +/* + * SubgraphPreconditioner.h + * Created on: Dec 31, 2009 + * @author: Frank Dellaert + */ + +#ifndef SUBGRAPHPRECONDITIONER_H_ +#define SUBGRAPHPRECONDITIONER_H_ + +#include "GaussianFactorGraph.h" +#include "GaussianBayesNet.h" + +namespace gtsam { + + /** + * Subgraph conditioner class, as explained in the RSS 2010 submission. + * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 + * We solve R1*x=c1, and make the substitution y=R1*x-c1. + * To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2. + * Then solve for yhat using CG, and solve for xhat = system.x(yhat). + */ + class SubgraphPreconditioner { + + private: + + const GaussianBayesNet& Rc1_; + + const GaussianFactorGraph& Ab2_; + const VectorConfig& xbar_; + const Errors b2bar_; /** b2 - A2*xbar */ + + public: + + /** + * Constructor + * @param Rc1: the Bayes Net R1*x=c1 + * @param Ab2: the Graph A2*x=b2 + * @param xbar: the solution to R1*x=c1 + */ + SubgraphPreconditioner(const GaussianBayesNet& Rc1, + const GaussianFactorGraph& Ab2, const VectorConfig& xbar); + + /* x = xbar + inv(R1)*y */ + VectorConfig x(const VectorConfig& y) const; + + /* error, given y */ + double error(const VectorConfig& y) const; + + /** gradient */ + VectorConfig gradient(const VectorConfig& y) const; + + /** Apply operator A */ + Errors operator*(const VectorConfig& y) const; + + /** Apply operator A' */ + VectorConfig operator^(const Errors& e) const; + }; + +} // nsamespace gtsam + +#endif /* SUBGRAPHPRECONDITIONER_H_ */ diff --git a/cpp/testBayesNetPreconditioner.cpp b/cpp/testBayesNetPreconditioner.cpp new file mode 100644 index 000000000..a5a4e9974 --- /dev/null +++ b/cpp/testBayesNetPreconditioner.cpp @@ -0,0 +1,112 @@ +/** + * @file testBayesNetConditioner.cpp + * @brief Unit tests for BayesNetConditioner + * @author Frank Dellaert + **/ + +#include +#include +#include + +#include "Ordering.h" +#include "smallExample.h" +#include "BayesNetPreconditioner.h" +#include "iterative-inl.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( BayesNetPreconditioner, operators ) +{ + // Build a simple Bayes net + // small Bayes Net x <- y, x=2D, y=1D + // 1 2 3 x1 0 + // 0 1 2 * x2 = 0 + // 0 0 1 x3 1 + + // Create a scalar Gaussian on y + GaussianBayesNet bn = scalarGaussian("y", 1, 0.1); + + // Add a conditional node with one parent |Rx+Sy-d| + Matrix R11 = Matrix_(2, 2, 1.0, 2.0, 0.0, 1.0), S12 = Matrix_(2, 1, 3.0, 2.0); + Vector d = zero(2); + Vector sigmas = Vector_(2, 0.1, 0.1); + push_front(bn, "x", d, R11, "y", S12, sigmas); + + // Create Precondioner class + GaussianFactorGraph dummy; + BayesNetPreconditioner P(dummy,bn); + + // inv(R1)*d should equal solution [1;-2;1] + VectorConfig D; + D.insert("x", d); + D.insert("y", Vector_(1, 1.0 / 0.1)); // corrected by sigma + VectorConfig expected1; + expected1.insert("x", Vector_(2, 1.0, -2.0)); + expected1.insert("y", Vector_(1, 1.0)); + VectorConfig actual1 = P.backSubstitute(D); + CHECK(assert_equal(expected1,actual1)); + + // inv(R1')*ones should equal ? + VectorConfig ones; + ones.insert("x", Vector_(2, 1.0, 1.0)); + ones.insert("y", Vector_(1, 1.0)); + VectorConfig expected2; + expected2.insert("x", Vector_(2, 0.1, -0.1)); + expected2.insert("y", Vector_(1, 0.0)); + VectorConfig actual2 = P.backSubstituteTranspose(ones); + CHECK(assert_equal(expected2,actual2)); +} + +/* ************************************************************************* */ +TEST( BayesNetPreconditioner, conjugateGradients ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorConfig xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); + + // Eliminate the spanning tree to build a prior + Ordering ordering = planarOrdering(N); + GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1 + VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1 + + // Create BayesNet-preconditioned system + BayesNetPreconditioner system(Ab,Rc1); + + // Create zero config y0 and perturbed config y1 + VectorConfig y0; + Vector z2 = zero(2); + BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2); + + VectorConfig y1 = y0; + y1.getReference("x23") = Vector_(2, 1.0, -1.0); + VectorConfig x1 = system.x(y1); + + // Solve using PCG + bool verbose = false; + double epsilon = 1e-6; // had to crank this down !!! + size_t maxIterations = 100; + VectorConfig actual_y = gtsam::conjugateGradients(system, y1, verbose, epsilon, maxIterations); + VectorConfig actual_x = system.x(actual_y); + CHECK(assert_equal(xtrue,actual_x)); + + // Compare with non preconditioned version: + VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon, + maxIterations); + CHECK(assert_equal(xtrue,actual2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/cpp/testSubgraphPreconditioner.cpp b/cpp/testSubgraphPreconditioner.cpp new file mode 100644 index 000000000..a67b24fd1 --- /dev/null +++ b/cpp/testSubgraphPreconditioner.cpp @@ -0,0 +1,212 @@ +/** + * @file testSubgraphConditioner.cpp + * @brief Unit tests for SubgraphPreconditioner + * @author Frank Dellaert + **/ + +#include +#include +#include +using namespace boost::assign; + +#include + +#include "numericalDerivative.h" +#include "Ordering.h" +#include "smallExample.h" +#include "SubgraphPreconditioner.h" +#include "iterative-inl.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( SubgraphPreconditioner, planarGraph ) +{ + // Check planar graph construction + GaussianFactorGraph A; + VectorConfig xtrue; + boost::tie(A, xtrue) = planarGraph(3); + LONGS_EQUAL(13,A.size()); + LONGS_EQUAL(9,xtrue.size()); + DOUBLES_EQUAL(0,A.error(xtrue),1e-9); // check zero error for xtrue + + // Check canonical ordering + Ordering expected, ordering = planarOrdering(3); + expected += "x33", "x23", "x13", "x32", "x22", "x12", "x31", "x21", "x11"; + CHECK(assert_equal(expected,ordering)); + + // Check that xtrue is optimal + GaussianBayesNet R1 = A.eliminate(ordering); + VectorConfig actual = optimize(R1); + CHECK(assert_equal(xtrue,actual)); +} + +/* ************************************************************************* */ +TEST( SubgraphPreconditioner, splitOffPlanarTree ) +{ + // Build a planar graph + GaussianFactorGraph A; + VectorConfig xtrue; + boost::tie(A, xtrue) = planarGraph(3); + + // Get the spanning tree and constraints, and check their sizes + GaussianFactorGraph T, C; + boost::tie(T, C) = splitOffPlanarTree(3, A); + LONGS_EQUAL(9,T.size()); + LONGS_EQUAL(4,C.size()); + + // Check that the tree can be solved to give the ground xtrue + Ordering ordering = planarOrdering(3); + GaussianBayesNet R1 = T.eliminate(ordering); + VectorConfig xbar = optimize(R1); + CHECK(assert_equal(xtrue,xbar)); +} + +/* ************************************************************************* */ +double error(const VectorConfig& x) { + // Build a planar graph + GaussianFactorGraph Ab; + VectorConfig xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); + + // Eliminate the spanning tree to build a prior + Ordering ordering = planarOrdering(N); + GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1 + VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1 + + SubgraphPreconditioner system(Rc1, Ab2, xbar); + return system.error(x); +} + +/* ************************************************************************* */ +TEST( SubgraphPreconditioner, system ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorConfig xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); + + // Eliminate the spanning tree to build a prior + Ordering ordering = planarOrdering(N); + GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1 + VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + SubgraphPreconditioner system(Rc1, Ab2, xbar); + + // Create zero config + VectorConfig zeros; + Vector z2 = zero(2); + BOOST_FOREACH(const string& j, ordering) zeros.insert(j,z2); + + // Set up y0 as all zeros + VectorConfig y0 = zeros; + + // y1 = perturbed y0 + VectorConfig y1 = zeros; + y1.getReference("x23") = Vector_(2, 1.0, -1.0); + + // Check corresponding x values + VectorConfig expected_x1 = xtrue, x1 = system.x(y1); + expected_x1.getReference("x23") = Vector_(2, 2.01, 2.99); + expected_x1.getReference("x33") = Vector_(2, 3.01, 2.99); + CHECK(assert_equal(xtrue, system.x(y0))); + CHECK(assert_equal(expected_x1,system.x(y1))); + + // Check errors + DOUBLES_EQUAL(0,Ab.error(xtrue),1e-9); + DOUBLES_EQUAL(3,Ab.error(x1),1e-9); + DOUBLES_EQUAL(0,system.error(y0),1e-9); + DOUBLES_EQUAL(3,system.error(y1),1e-9); + + // Test gradient in x + VectorConfig expected_gx0 = zeros; + VectorConfig expected_gx1 = zeros; + CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue))); + expected_gx1.getReference("x13") = Vector_(2, -100., 100.); + expected_gx1.getReference("x22") = Vector_(2, -100., 100.); + expected_gx1.getReference("x23") = Vector_(2, 200., -200.); + expected_gx1.getReference("x32") = Vector_(2, -100., 100.); + expected_gx1.getReference("x33") = Vector_(2, 100., -100.); + CHECK(assert_equal(expected_gx1,Ab.gradient(x1))); + + // Test gradient in y + VectorConfig expected_gy0 = zeros; + VectorConfig expected_gy1 = zeros; + expected_gy1.getReference("x13") = Vector_(2, 2., -2.); + expected_gy1.getReference("x22") = Vector_(2, -2., 2.); + expected_gy1.getReference("x23") = Vector_(2, 3., -3.); + expected_gy1.getReference("x32") = Vector_(2, -1., 1.); + expected_gy1.getReference("x33") = Vector_(2, 1., -1.); + CHECK(assert_equal(expected_gy0,system.gradient(y0))); + CHECK(assert_equal(expected_gy1,system.gradient(y1))); + + // Check it numerically for good measure + // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) + Vector numerical_g1 = numericalGradient (error, y1, 0.001); + Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., + 3., -3., 0., 0., -1., 1., 1., -1.); + CHECK(assert_equal(expected_g1,numerical_g1)); +} + +/* ************************************************************************* */ +TEST( SubgraphPreconditioner, conjugateGradients ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorConfig xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); + + // Eliminate the spanning tree to build a prior + Ordering ordering = planarOrdering(N); + GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1 + VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + SubgraphPreconditioner system(Rc1, Ab2, xbar); + + // Create zero config y0 and perturbed config y1 + VectorConfig y0; + Vector z2 = zero(2); + BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2); + + VectorConfig y1 = y0; + y1.getReference("x23") = Vector_(2, 1.0, -1.0); + VectorConfig x1 = system.x(y1); + + // Solve for the remaining constraints using PCG + bool verbose = false; + double epsilon = 1e-3; + size_t maxIterations = 100; + VectorConfig actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, maxIterations); + CHECK(assert_equal(y0,actual)); + + // Compare with non preconditioned version: + VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon, + maxIterations); + CHECK(assert_equal(xtrue,actual2,1e-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */