diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 7b59735d9..8c1fcc837 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -48,6 +48,7 @@ namespace gtsam { public: SubgraphPreconditioner(); + /** * Constructor * @param Ab1: the Graph A1*x=b1 @@ -55,7 +56,8 @@ namespace gtsam { * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); + SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, + const sharedBayesNet& Rc1, const sharedValues& xbar); /** Access Ab1 */ const sharedFG& Ab1() const { return Ab1_; } @@ -69,23 +71,23 @@ namespace gtsam { /** Access b2bar */ const sharedErrors b2bar() const { return b2bar_; } - /** - * Add zero-mean i.i.d. Gaussian prior terms to each variable - * @param sigma Standard deviation of Gaussian - */ -// SubgraphPreconditioner add_priors(double sigma) const; + /** + * Add zero-mean i.i.d. Gaussian prior terms to each variable + * @param sigma Standard deviation of Gaussian + */ +// SubgraphPreconditioner add_priors(double sigma) const; /* x = xbar + inv(R1)*y */ VectorValues x(const VectorValues& y) const; /* A zero VectorValues with the structure of xbar */ VectorValues zero() const { - VectorValues V(VectorValues::Zero(*xbar_)) ; + VectorValues V(VectorValues::Zero(*xbar_)); return V ; } /** - * Add constraint part of the error only, used in both calls above + * Add constraint part of the error only * y += alpha*inv(R1')*A2'*e2 * Takes a range indicating e2 !!!! */ diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 8a052a66f..5cc0841b0 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -122,7 +122,7 @@ namespace gtsam { // conjugate gradient method. // S: linear system, V: step vector, E: errors template - V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters ¶meters, bool steepest = false) { + V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters ¶meters, bool steepest) { CGState state(Ab, x, parameters, steepest); diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index 6dadb2d72..d0a7fd551 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -22,50 +22,57 @@ #include #include - #include using namespace std; namespace gtsam { - /* ************************************************************************* */ - void System::print (const string& s) const { - cout << s << endl; - gtsam::print(A_, "A"); - gtsam::print(b_, "b"); - } + /* ************************************************************************* */ + void System::print(const string& s) const { + cout << s << endl; + gtsam::print(A_, "A"); + gtsam::print(b_, "b"); + } - /* ************************************************************************* */ + /* ************************************************************************* */ - Vector steepestDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients (Ab, x, parameters, true); - } + Vector steepestDescent(const System& Ab, const Vector& x, + const ConjugateGradientParameters & parameters) { + return conjugateGradients(Ab, x, parameters, true); + } - Vector conjugateGradientDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients (Ab, x, parameters); - } + Vector conjugateGradientDescent(const System& Ab, const Vector& x, + const ConjugateGradientParameters & parameters) { + return conjugateGradients(Ab, x, parameters); + } - /* ************************************************************************* */ - Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { - System Ab(A, b); - return conjugateGradients (Ab, x, parameters, true); - } + /* ************************************************************************* */ + Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, + const ConjugateGradientParameters & parameters) { + System Ab(A, b); + return conjugateGradients(Ab, x, parameters, true); + } - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { - System Ab(A, b); - return conjugateGradients (Ab, x, parameters); - } + Vector conjugateGradientDescent(const Matrix& A, const Vector& b, + const Vector& x, const ConjugateGradientParameters & parameters) { + System Ab(A, b); + return conjugateGradients(Ab, x, parameters); + } - /* ************************************************************************* */ - VectorValues steepestDescent(const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors> (fg, x, parameters, true); - } + /* ************************************************************************* */ + VectorValues steepestDescent(const FactorGraph& fg, + const VectorValues& x, const ConjugateGradientParameters & parameters) { + return conjugateGradients, VectorValues, Errors>( + fg, x, parameters, true); + } - VectorValues conjugateGradientDescent(const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors> (fg, x, parameters); - } + VectorValues conjugateGradientDescent(const FactorGraph& fg, + const VectorValues& x, const ConjugateGradientParameters & parameters) { + return conjugateGradients, VectorValues, Errors>( + fg, x, parameters); + } - /* ************************************************************************* */ +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 5ffb366c2..384dfe814 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -31,12 +31,11 @@ namespace gtsam { * "Vector" class E needs dot(v,v) * @param Ab, the "system" that needs to be solved, examples below * @param x is the initial estimate - * @param epsilon determines the convergence criterion: norm(g) - V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, size_t maxIterations, bool steepest = false); + template + V conjugateGradients(const S& Ab, V x, + const ConjugateGradientParameters ¶meters, bool steepest = false); /** * Helper class encapsulating the combined system |Ax-b_|^2 @@ -127,21 +126,19 @@ namespace gtsam { const Vector& x, const ConjugateGradientParameters & parameters); - class GaussianFactorGraph; - /** * Method of steepest gradients, Gaussian Factor Graph version - * */ + */ VectorValues steepestDescent( - const GaussianFactorGraph& fg, + const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); /** * Method of conjugate gradients (CG), Gaussian Factor Graph version - * */ + */ VectorValues conjugateGradientDescent( - const GaussianFactorGraph& fg, + const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index 100a4db4c..f81ab5528 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -421,7 +421,7 @@ namespace example { } /* ************************************************************************* */ - boost::tuple planarGraph(size_t N) { + boost::tuple planarGraph(size_t N) { // create empty graph NonlinearFactorGraph nlfg; @@ -458,7 +458,13 @@ namespace example { xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); // linearize around zero - return boost::make_tuple(*nlfg.linearize(zeros, ordering), xtrue); + boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + + JacobianFactorGraph jfg; + BOOST_FOREACH(GaussianFactorGraph::sharedFactor factor, *gfg) + jfg.push_back(boost::dynamic_pointer_cast(factor)); + + return boost::make_tuple(jfg, xtrue); } /* ************************************************************************* */ @@ -476,21 +482,21 @@ namespace example { JacobianFactorGraph T, C; // Add the x11 constraint to the tree - T.push_back(original[0]); + T.push_back(boost::dynamic_pointer_cast(original[0])); // Add all horizontal constraints to the tree size_t i = 1; for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++, i++) - T.push_back(original[i]); + T.push_back(boost::dynamic_pointer_cast(original[i])); // Add first vertical column of constraints to T, others to C for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++, i++) if (x == 1) - T.push_back(original[i]); + T.push_back(boost::dynamic_pointer_cast(original[i])); else - C.push_back(original[i]); + C.push_back(boost::dynamic_pointer_cast(original[i])); return make_pair(T, C); } diff --git a/tests/smallExample.h b/tests/smallExample.h index 161c45da8..54c3a14c5 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -130,7 +130,7 @@ namespace gtsam { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ - boost::tuple planarGraph(size_t N); + boost::tuple planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 417643e78..60f737d67 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -35,30 +36,41 @@ using namespace gtsam; using namespace example; // define keys -Key i3003 = 3003, i2003 = 2003, i1003 = 1003; -Key i3002 = 3002, i2002 = 2002, i1002 = 1002; -Key i3001 = 3001, i2001 = 2001, i1001 = 1001; +// Create key for simulated planar graph +Symbol key(int x, int y) { + return symbol_shorthand::X(1000*x+y); +} -// TODO fix Ordering::equals, because the ordering *is* correct ! /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, planarOrdering ) -//{ -// // Check canonical ordering -// Ordering expected, ordering = planarOrdering(3); -// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; -// CHECK(assert_equal(expected,ordering)); -//} +TEST( SubgraphPreconditioner, planarOrdering ) { + // Check canonical ordering + Ordering expected, ordering = planarOrdering(3); + expected += + key(3, 3), key(2, 3), key(1, 3), + key(3, 2), key(2, 2), key(1, 2), + key(3, 1), key(2, 1), key(1, 1); + CHECK(assert_equal(expected,ordering)); +} + +/* ************************************************************************* */ +/** unnormalized error */ +double error(const JacobianFactorGraph& fg, const VectorValues& x) { + double total_error = 0.; + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) + total_error += factor->error(x); + return total_error; +} /* ************************************************************************* */ TEST( SubgraphPreconditioner, planarGraph ) { // Check planar graph construction - GaussianFactorGraph A; + JacobianFactorGraph A; VectorValues 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 + DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue // Check that xtrue is optimal GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); @@ -67,143 +79,143 @@ TEST( SubgraphPreconditioner, planarGraph ) } /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, splitOffPlanarTree ) -//{ -// // Build a planar graph -// GaussianFactorGraph A; -// VectorValues xtrue; -// boost::tie(A, xtrue) = planarGraph(3); -// -// // Get the spanning tree and constraints, and check their sizes -// JacobianFactorGraph T, C; -// // TODO big mess: GFG and JFG mess !!! -// 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 -// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); -// VectorValues xbar = optimize(*R1); -// CHECK(assert_equal(xtrue,xbar)); -//} +TEST( SubgraphPreconditioner, splitOffPlanarTree ) +{ + // Build a planar graph + JacobianFactorGraph A; + VectorValues xtrue; + boost::tie(A, xtrue) = planarGraph(3); + + // Get the spanning tree and constraints, and check their sizes + JacobianFactorGraph 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 + GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); + VectorValues xbar = optimize(*R1); + CHECK(assert_equal(xtrue,xbar)); +} /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, system ) -//{ -// // Build a planar graph -// JacobianFactorGraph Ab; -// VectorValues 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); -// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); -// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); -// -// // Eliminate the spanning tree to build a prior -// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 -// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 -// -// // Create Subgraph-preconditioned system -// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); -// -// // Create zero config -// VectorValues zeros = VectorValues::Zero(xbar); -// -// // Set up y0 as all zeros -// VectorValues y0 = zeros; -// -// // y1 = perturbed y0 -// VectorValues y1 = zeros; -// y1[i2003] = Vector_(2, 1.0, -1.0); -// -// // Check corresponding x values -// VectorValues expected_x1 = xtrue, x1 = system.x(y1); -// expected_x1[i2003] = Vector_(2, 2.01, 2.99); -// expected_x1[i3003] = 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,error(Ab,xtrue),1e-9); // TODO ! -//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! -// DOUBLES_EQUAL(0,error(system,y0),1e-9); -// DOUBLES_EQUAL(3,error(system,y1),1e-9); -// -// // Test gradient in x -// VectorValues expected_gx0 = zeros; -// VectorValues expected_gx1 = zeros; -// CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); -// expected_gx1[i1003] = Vector_(2, -100., 100.); -// expected_gx1[i2002] = Vector_(2, -100., 100.); -// expected_gx1[i2003] = Vector_(2, 200., -200.); -// expected_gx1[i3002] = Vector_(2, -100., 100.); -// expected_gx1[i3003] = Vector_(2, 100., -100.); -// CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); -// -// // Test gradient in y -// VectorValues expected_gy0 = zeros; -// VectorValues expected_gy1 = zeros; -// expected_gy1[i1003] = Vector_(2, 2., -2.); -// expected_gy1[i2002] = Vector_(2, -2., 2.); -// expected_gy1[i2003] = Vector_(2, 3., -3.); -// expected_gy1[i3002] = Vector_(2, -1., 1.); -// expected_gy1[i3003] = Vector_(2, 1., -1.); -// CHECK(assert_equal(expected_gy0,gradient(system,y0))); -// CHECK(assert_equal(expected_gy1,gradient(system,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, system ) +{ + // Build a planar graph + JacobianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + + // Eliminate the spanning tree to build a prior + SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); + + // Create zero config + VectorValues zeros = VectorValues::Zero(xbar); + + // Set up y0 as all zeros + VectorValues y0 = zeros; + + // y1 = perturbed y0 + VectorValues y1 = zeros; + y1[1] = Vector_(2, 1.0, -1.0); + + // Check corresponding x values + VectorValues expected_x1 = xtrue, x1 = system.x(y1); + expected_x1[1] = Vector_(2, 2.01, 2.99); + expected_x1[0] = 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,error(Ab,xtrue),1e-9); + DOUBLES_EQUAL(3,error(Ab,x1),1e-9); + DOUBLES_EQUAL(0,error(system,y0),1e-9); + DOUBLES_EQUAL(3,error(system,y1),1e-9); + + // Test gradient in x + VectorValues expected_gx0 = zeros; + VectorValues expected_gx1 = zeros; + CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); + expected_gx1[2] = Vector_(2, -100., 100.); + expected_gx1[4] = Vector_(2, -100., 100.); + expected_gx1[1] = Vector_(2, 200., -200.); + expected_gx1[3] = Vector_(2, -100., 100.); + expected_gx1[0] = Vector_(2, 100., -100.); + CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); + + // Test gradient in y + VectorValues expected_gy0 = zeros; + VectorValues expected_gy1 = zeros; + expected_gy1[2] = Vector_(2, 2., -2.); + expected_gy1[4] = Vector_(2, -2., 2.); + expected_gy1[1] = Vector_(2, 3., -3.); + expected_gy1[3] = Vector_(2, -1., 1.); + expected_gy1[0] = Vector_(2, 1., -1.); + CHECK(assert_equal(expected_gy0,gradient(system,y0))); + CHECK(assert_equal(expected_gy1,gradient(system,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; -// VectorValues 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); -// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); -// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); -// -// // Eliminate the spanning tree to build a prior -// Ordering ordering = planarOrdering(N); -// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 -// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 -// -// // Create Subgraph-preconditioned system -// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); -// -// // Create zero config y0 and perturbed config y1 -// VectorValues y0 = VectorValues::Zero(xbar); -// -// VectorValues y1 = y0; -// y1[i2003] = Vector_(2, 1.0, -1.0); -// VectorValues x1 = system.x(y1); -// -// // Solve for the remaining constraints using PCG -// ConjugateGradientParameters parameters; -//// VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); -//// CHECK(assert_equal(y0,actual)); -// -// // Compare with non preconditioned version: -// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); -// CHECK(assert_equal(xtrue,actual2,1e-4)); -//} +TEST( SubgraphPreconditioner, conjugateGradients ) +{ + // Build a planar graph + JacobianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + + // Eliminate the spanning tree to build a prior + Ordering ordering = planarOrdering(N); + SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); + + // Create zero config y0 and perturbed config y1 + VectorValues y0 = VectorValues::Zero(xbar); + + VectorValues y1 = y0; + y1[1] = Vector_(2, 1.0, -1.0); + VectorValues x1 = system.x(y1); + + // Solve for the remaining constraints using PCG + ConjugateGradientParameters parameters; + VectorValues actual = conjugateGradients(system, y1, parameters); + CHECK(assert_equal(y0,actual)); + + // Compare with non preconditioned version: + VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); + CHECK(assert_equal(xtrue,actual2,1e-4)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }