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

Made SPCG unit tests compile again, needed several fixes in iterative.h

release/4.3a0
Frank Dellaert 2012-09-03 00:06:13 +00:00
parent a058ecb692
commit ab7594e8f0
7 changed files with 227 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/linear/SubgraphPreconditioner.h
View File

@ -48,6 +48,7 @@ namespace gtsam {
public: public:
SubgraphPreconditioner(); SubgraphPreconditioner();
/** /**
* Constructor * Constructor
* @param Ab1: the Graph A1*x=b1 * @param Ab1: the Graph A1*x=b1
@ -55,7 +56,8 @@ namespace gtsam {
* @param Rc1: the Bayes Net R1*x=c1 * @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to 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 */ /** Access Ab1 */
const sharedFG& Ab1() const { return Ab1_; } const sharedFG& Ab1() const { return Ab1_; }
@ -69,23 +71,23 @@ namespace gtsam {
/** Access b2bar */ /** Access b2bar */
const sharedErrors b2bar() const { return b2bar_; } const sharedErrors b2bar() const { return b2bar_; }
/** /**
* Add zero-mean i.i.d. Gaussian prior terms to each variable * Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian * @param sigma Standard deviation of Gaussian
*/ */
// SubgraphPreconditioner add_priors(double sigma) const; // SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */ /* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const; VectorValues x(const VectorValues& y) const;
/* A zero VectorValues with the structure of xbar */ /* A zero VectorValues with the structure of xbar */
VectorValues zero() const { VectorValues zero() const {
VectorValues V(VectorValues::Zero(*xbar_)) ; VectorValues V(VectorValues::Zero(*xbar_));
return V ; 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 * y += alpha*inv(R1')*A2'*e2
* Takes a range indicating e2 !!!! * Takes a range indicating e2 !!!!
*/ */

2
gtsam/linear/iterative-inl.h
View File

@ -122,7 +122,7 @@ namespace gtsam {
// conjugate gradient method. // conjugate gradient method.
// S: linear system, V: step vector, E: errors // S: linear system, V: step vector, E: errors
template<class S, class V, class E> template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters &parameters, bool steepest = false) { V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters &parameters, bool steepest) {
CGState<S, V, E> state(Ab, x, parameters, steepest); CGState<S, V, E> state(Ab, x, parameters, steepest);

69
gtsam/linear/iterative.cpp
View File

@ -22,50 +22,57 @@
#include <gtsam/linear/JacobianFactorGraph.h> #include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h> #include <gtsam/linear/IterativeSolver.h>
#include <iostream> #include <iostream>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void System::print (const string& s) const { void System::print(const string& s) const {
cout << s << endl; cout << s << endl;
gtsam::print(A_, "A"); gtsam::print(A_, "A");
gtsam::print(b_, "b"); gtsam::print(b_, "b");
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector steepestDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { Vector steepestDescent(const System& Ab, const Vector& x,
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true); const ConjugateGradientParameters & parameters) {
} return conjugateGradients<System, Vector, Vector>(Ab, x, parameters, true);
}
Vector conjugateGradientDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { Vector conjugateGradientDescent(const System& Ab, const Vector& x,
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters); const ConjugateGradientParameters & parameters) {
} return conjugateGradients<System, Vector, Vector>(Ab, x, parameters);
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
System Ab(A, b); const ConjugateGradientParameters & parameters) {
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true); System Ab(A, b);
} return conjugateGradients<System, Vector, Vector>(Ab, x, parameters, true);
}
Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
System Ab(A, b); const Vector& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters); System Ab(A, b);
} return conjugateGradients<System, Vector, Vector>(Ab, x, parameters);
}
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues steepestDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { VectorValues steepestDescent(const FactorGraph<JacobianFactor>& fg,
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters, true); const VectorValues& x, const ConjugateGradientParameters & parameters) {
} return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors>(
fg, x, parameters, true);
}
VectorValues conjugateGradientDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { VectorValues conjugateGradientDescent(const FactorGraph<JacobianFactor>& fg,
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters); const VectorValues& x, const ConjugateGradientParameters & parameters) {
} return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors>(
fg, x, parameters);
}
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

17
gtsam/linear/iterative.h
View File

@ -31,12 +31,11 @@ namespace gtsam {
* "Vector" class E needs dot(v,v) * "Vector" class E needs dot(v,v)
* @param Ab, the "system" that needs to be solved, examples below * @param Ab, the "system" that needs to be solved, examples below
* @param x is the initial estimate * @param x is the initial estimate
* @param epsilon determines the convergence criterion: norm(g)<epsilon*norm(g0)
* @param maxIterations, if 0 will be set to |x|
* @param steepest flag, if true does steepest descent, not CG * @param steepest flag, if true does steepest descent, not CG
* */ * */
template<class S, class V, class E> template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, size_t maxIterations, bool steepest = false); V conjugateGradients(const S& Ab, V x,
const ConjugateGradientParameters &parameters, bool steepest = false);
/** /**
* Helper class encapsulating the combined system |Ax-b_|^2 * Helper class encapsulating the combined system |Ax-b_|^2
@ -127,21 +126,19 @@ namespace gtsam {
const Vector& x, const Vector& x,
const ConjugateGradientParameters & parameters); const ConjugateGradientParameters & parameters);
class GaussianFactorGraph;
/** /**
* Method of steepest gradients, Gaussian Factor Graph version * Method of steepest gradients, Gaussian Factor Graph version
* */ */
VectorValues steepestDescent( VectorValues steepestDescent(
const GaussianFactorGraph& fg, const FactorGraph<JacobianFactor>& fg,
const VectorValues& x, const VectorValues& x,
const ConjugateGradientParameters & parameters); const ConjugateGradientParameters & parameters);
/** /**
* Method of conjugate gradients (CG), Gaussian Factor Graph version * Method of conjugate gradients (CG), Gaussian Factor Graph version
* */ */
VectorValues conjugateGradientDescent( VectorValues conjugateGradientDescent(
const GaussianFactorGraph& fg, const FactorGraph<JacobianFactor>& fg,
const VectorValues& x, const VectorValues& x,
const ConjugateGradientParameters & parameters); const ConjugateGradientParameters & parameters);

18
tests/smallExample.cpp
View File

@ -421,7 +421,7 @@ namespace example {
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) { boost::tuple<JacobianFactorGraph, VectorValues> planarGraph(size_t N) {
// create empty graph // create empty graph
NonlinearFactorGraph nlfg; NonlinearFactorGraph nlfg;
@ -458,7 +458,13 @@ namespace example {
xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
// linearize around zero // linearize around zero
return boost::make_tuple(*nlfg.linearize(zeros, ordering), xtrue); boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
JacobianFactorGraph jfg;
BOOST_FOREACH(GaussianFactorGraph::sharedFactor factor, *gfg)
jfg.push_back(boost::dynamic_pointer_cast<JacobianFactor>(factor));
return boost::make_tuple(jfg, xtrue);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -476,21 +482,21 @@ namespace example {
JacobianFactorGraph T, C; JacobianFactorGraph T, C;
// Add the x11 constraint to the tree // Add the x11 constraint to the tree
T.push_back(original[0]); T.push_back(boost::dynamic_pointer_cast<JacobianFactor>(original[0]));
// Add all horizontal constraints to the tree // Add all horizontal constraints to the tree
size_t i = 1; size_t i = 1;
for (size_t x = 1; x < N; x++) for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++, i++) for (size_t y = 1; y <= N; y++, i++)
T.push_back(original[i]); T.push_back(boost::dynamic_pointer_cast<JacobianFactor>(original[i]));
// Add first vertical column of constraints to T, others to C // Add first vertical column of constraints to T, others to C
for (size_t x = 1; x <= N; x++) for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++, i++) for (size_t y = 1; y < N; y++, i++)
if (x == 1) if (x == 1)
T.push_back(original[i]); T.push_back(boost::dynamic_pointer_cast<JacobianFactor>(original[i]));
else else
C.push_back(original[i]); C.push_back(boost::dynamic_pointer_cast<JacobianFactor>(original[i]));
return make_pair(T, C); return make_pair(T, C);
} }

2
tests/smallExample.h
View File

@ -130,7 +130,7 @@ namespace gtsam {
* -x11-x21-x31 * -x11-x21-x31
* with x11 clamped at (1,1), and others related by 2D odometry. * with x11 clamped at (1,1), and others related by 2D odometry.
*/ */
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N); boost::tuple<JacobianFactorGraph, VectorValues> planarGraph(size_t N);
/* /*
* Create canonical ordering for planar graph that also works for tree * Create canonical ordering for planar graph that also works for tree

304
tests/testSubgraphPreconditioner.cpp
View File

@ -17,6 +17,7 @@
#include <tests/smallExample.h> #include <tests/smallExample.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/iterative.h> #include <gtsam/linear/iterative.h>
#include <gtsam/linear/JacobianFactorGraph.h> #include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
@ -35,30 +36,41 @@ using namespace gtsam;
using namespace example; using namespace example;
// define keys // define keys
Key i3003 = 3003, i2003 = 2003, i1003 = 1003; // Create key for simulated planar graph
Key i3002 = 3002, i2002 = 2002, i1002 = 1002; Symbol key(int x, int y) {
Key i3001 = 3001, i2001 = 2001, i1001 = 1001; return symbol_shorthand::X(1000*x+y);
}
// TODO fix Ordering::equals, because the ordering *is* correct !
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( SubgraphPreconditioner, planarOrdering ) TEST( SubgraphPreconditioner, planarOrdering ) {
//{ // Check canonical ordering
// // Check canonical ordering Ordering expected, ordering = planarOrdering(3);
// Ordering expected, ordering = planarOrdering(3); expected +=
// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; key(3, 3), key(2, 3), key(1, 3),
// CHECK(assert_equal(expected,ordering)); 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 ) TEST( SubgraphPreconditioner, planarGraph )
{ {
// Check planar graph construction // Check planar graph construction
GaussianFactorGraph A; JacobianFactorGraph A;
VectorValues xtrue; VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3); boost::tie(A, xtrue) = planarGraph(3);
LONGS_EQUAL(13,A.size()); LONGS_EQUAL(13,A.size());
LONGS_EQUAL(9,xtrue.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 // Check that xtrue is optimal
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
@ -67,143 +79,143 @@ TEST( SubgraphPreconditioner, planarGraph )
} }
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( SubgraphPreconditioner, splitOffPlanarTree ) TEST( SubgraphPreconditioner, splitOffPlanarTree )
//{ {
// // Build a planar graph // Build a planar graph
// GaussianFactorGraph A; JacobianFactorGraph A;
// VectorValues xtrue; VectorValues xtrue;
// boost::tie(A, xtrue) = planarGraph(3); boost::tie(A, xtrue) = planarGraph(3);
//
// // Get the spanning tree and constraints, and check their sizes // Get the spanning tree and constraints, and check their sizes
// JacobianFactorGraph T, C; JacobianFactorGraph T, C;
// // TODO big mess: GFG and JFG mess !!! boost::tie(T, C) = splitOffPlanarTree(3, A);
// boost::tie(T, C) = splitOffPlanarTree(3, A); LONGS_EQUAL(9,T.size());
// LONGS_EQUAL(9,T.size()); LONGS_EQUAL(4,C.size());
// LONGS_EQUAL(4,C.size());
// // Check that the tree can be solved to give the ground xtrue
// // Check that the tree can be solved to give the ground xtrue GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); VectorValues xbar = optimize(*R1);
// VectorValues xbar = optimize(*R1); CHECK(assert_equal(xtrue,xbar));
// CHECK(assert_equal(xtrue,xbar)); }
//}
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( SubgraphPreconditioner, system )
//{ TEST( SubgraphPreconditioner, system )
// // Build a planar graph {
// JacobianFactorGraph Ab; // Build a planar graph
// VectorValues xtrue; JacobianFactorGraph Ab;
// size_t N = 3; VectorValues xtrue;
// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b 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 // Get the spanning tree and corresponding ordering
// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); 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 // Eliminate the spanning tree to build a prior
// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 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 // Create Subgraph-preconditioned system
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
// // Create zero config
// VectorValues zeros = VectorValues::Zero(xbar); // Create zero config
// VectorValues zeros = VectorValues::Zero(xbar);
// // Set up y0 as all zeros
// VectorValues y0 = zeros; // Set up y0 as all zeros
// VectorValues y0 = zeros;
// // y1 = perturbed y0
// VectorValues y1 = zeros; // y1 = perturbed y0
// y1[i2003] = Vector_(2, 1.0, -1.0); VectorValues y1 = zeros;
// y1[1] = Vector_(2, 1.0, -1.0);
// // Check corresponding x values
// VectorValues expected_x1 = xtrue, x1 = system.x(y1); // Check corresponding x values
// expected_x1[i2003] = Vector_(2, 2.01, 2.99); VectorValues expected_x1 = xtrue, x1 = system.x(y1);
// expected_x1[i3003] = Vector_(2, 3.01, 2.99); expected_x1[1] = Vector_(2, 2.01, 2.99);
// CHECK(assert_equal(xtrue, system.x(y0))); expected_x1[0] = Vector_(2, 3.01, 2.99);
// CHECK(assert_equal(expected_x1,system.x(y1))); 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 ! // Check errors
//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9);
// DOUBLES_EQUAL(0,error(system,y0),1e-9); DOUBLES_EQUAL(3,error(Ab,x1),1e-9);
// DOUBLES_EQUAL(3,error(system,y1),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; // Test gradient in x
// VectorValues expected_gx1 = zeros; VectorValues expected_gx0 = zeros;
// CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); VectorValues expected_gx1 = zeros;
// expected_gx1[i1003] = Vector_(2, -100., 100.); CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
// expected_gx1[i2002] = Vector_(2, -100., 100.); expected_gx1[2] = Vector_(2, -100., 100.);
// expected_gx1[i2003] = Vector_(2, 200., -200.); expected_gx1[4] = Vector_(2, -100., 100.);
// expected_gx1[i3002] = Vector_(2, -100., 100.); expected_gx1[1] = Vector_(2, 200., -200.);
// expected_gx1[i3003] = Vector_(2, 100., -100.); expected_gx1[3] = Vector_(2, -100., 100.);
// CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); expected_gx1[0] = Vector_(2, 100., -100.);
// CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
// // Test gradient in y
// VectorValues expected_gy0 = zeros; // Test gradient in y
// VectorValues expected_gy1 = zeros; VectorValues expected_gy0 = zeros;
// expected_gy1[i1003] = Vector_(2, 2., -2.); VectorValues expected_gy1 = zeros;
// expected_gy1[i2002] = Vector_(2, -2., 2.); expected_gy1[2] = Vector_(2, 2., -2.);
// expected_gy1[i2003] = Vector_(2, 3., -3.); expected_gy1[4] = Vector_(2, -2., 2.);
// expected_gy1[i3002] = Vector_(2, -1., 1.); expected_gy1[1] = Vector_(2, 3., -3.);
// expected_gy1[i3003] = Vector_(2, 1., -1.); expected_gy1[3] = Vector_(2, -1., 1.);
// CHECK(assert_equal(expected_gy0,gradient(system,y0))); expected_gy1[0] = Vector_(2, 1., -1.);
// CHECK(assert_equal(expected_gy1,gradient(system,y1))); 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) // Check it numerically for good measure
// // Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001); // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
// // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., // Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
// // 3., -3., 0., 0., -1., 1., 1., -1.); // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
// // CHECK(assert_equal(expected_g1,numerical_g1)); // 3., -3., 0., 0., -1., 1., 1., -1.);
//} // CHECK(assert_equal(expected_g1,numerical_g1));
}
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( SubgraphPreconditioner, conjugateGradients ) TEST( SubgraphPreconditioner, conjugateGradients )
//{ {
// // Build a planar graph // Build a planar graph
// GaussianFactorGraph Ab; JacobianFactorGraph Ab;
// VectorValues xtrue; VectorValues xtrue;
// size_t N = 3; size_t N = 3;
// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
//
// // Get the spanning tree and corresponding ordering // Get the spanning tree and corresponding ordering
// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_));
// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_));
//
// // Eliminate the spanning tree to build a prior // Eliminate the spanning tree to build a prior
// Ordering ordering = planarOrdering(N); Ordering ordering = planarOrdering(N);
// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
//
// // Create Subgraph-preconditioned system // Create Subgraph-preconditioned system
// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
//
// // Create zero config y0 and perturbed config y1 // Create zero config y0 and perturbed config y1
// VectorValues y0 = VectorValues::Zero(xbar); VectorValues y0 = VectorValues::Zero(xbar);
//
// VectorValues y1 = y0; VectorValues y1 = y0;
// y1[i2003] = Vector_(2, 1.0, -1.0); y1[1] = Vector_(2, 1.0, -1.0);
// VectorValues x1 = system.x(y1); VectorValues x1 = system.x(y1);
//
// // Solve for the remaining constraints using PCG // Solve for the remaining constraints using PCG
// ConjugateGradientParameters parameters; ConjugateGradientParameters parameters;
//// VectorValues actual = gtsam::conjugateGradients<SubgraphPreconditioner, VectorValues actual = conjugateGradients<SubgraphPreconditioner,
//// VectorValues, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations); VectorValues, Errors>(system, y1, parameters);
//// CHECK(assert_equal(y0,actual)); CHECK(assert_equal(y0,actual));
//
// // Compare with non preconditioned version: // Compare with non preconditioned version:
// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
// CHECK(assert_equal(xtrue,actual2,1e-4)); CHECK(assert_equal(xtrue,actual2,1e-4));
//} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }