diff --git a/.cproject b/.cproject index 95c0f2a96..df6fdc5f5 100644 --- a/.cproject +++ b/.cproject @@ -309,14 +309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +335,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +343,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +391,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +399,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +407,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +423,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +525,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +541,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +581,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +685,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -985,6 +991,7 @@ make + testGraph.run true false @@ -992,6 +999,7 @@ make + testJunctionTree.run true false @@ -999,6 +1007,7 @@ make + testSymbolicBayesNetB.run true false @@ -1028,6 +1037,30 @@ true true + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + make -j5 @@ -1134,6 +1167,7 @@ make + testErrors.run true false @@ -1179,10 +1213,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1267,10 +1301,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1605,7 +1639,6 @@ make - testSimulated2DOriented.run true false @@ -1645,7 +1678,6 @@ make - testSimulated2D.run true false @@ -1653,7 +1685,6 @@ make - testSimulated3D.run true false @@ -1845,7 +1876,6 @@ make - tests/testGaussianISAM2 true false @@ -1867,102 +1897,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j1 @@ -2164,6 +2098,7 @@ cpack + -G DEB true false @@ -2171,6 +2106,7 @@ cpack + -G RPM true false @@ -2178,6 +2114,7 @@ cpack + -G TGZ true false @@ -2185,6 +2122,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2318,34 +2256,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2389,6 +2391,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index c34f7e22e..9794ba51e 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -101,7 +101,7 @@ boost::tuple SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); - const size_t n = index.size(), m = jfg.size(); + const size_t n = index.size(); DisjointSet D(n) ; GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index 6df106acf..afa878a01 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -138,9 +138,9 @@ namespace example { } /* ************************************************************************* */ - FactorGraph createGaussianFactorGraph(const Ordering& ordering) { + GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { // Create empty graph - FactorGraph fg; + GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); diff --git a/tests/smallExample.h b/tests/smallExample.h index 9c43563a3..f87847f24 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -65,7 +65,7 @@ namespace gtsam { * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ - FactorGraph createGaussianFactorGraph(const Ordering& ordering); + GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); /** * create small Chordal Bayes Net x <- y diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index a41c8d9a7..f5419b7ff 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -57,10 +57,11 @@ TEST( GaussianFactor, linearFactor ) JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2)); // create a small linear factor graph - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph - JacobianFactor::shared_ptr lf = fg[1]; + JacobianFactor::shared_ptr lf = + boost::dynamic_pointer_cast(fg[1]); // check if the two factors are the same EXPECT(assert_equal(expected,*lf)); @@ -72,7 +73,7 @@ TEST( GaussianFactor, getDim ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // get a factor Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); GaussianFactor::shared_ptr factor = fg[0]; // get the size of a variable @@ -89,7 +90,7 @@ TEST( GaussianFactor, error ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the first factor from the factor graph GaussianFactor::shared_ptr lf = fg[0]; @@ -109,7 +110,7 @@ TEST( GaussianFactor, matrix ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version @@ -158,7 +159,7 @@ TEST( GaussianFactor, matrix_aug ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; @@ -210,7 +211,7 @@ TEST( GaussianFactor, size ) // create a linear factor graph const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get some factors from the graph boost::shared_ptr factor1 = fg[0]; diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index ca8b9b1ea..306c64fb7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -369,7 +369,7 @@ TEST( GaussianFactorGraph, multiplication ) // create an ordering Ordering ord; ord += X(2),L(1),X(1); - FactorGraph A = createGaussianFactorGraph(ord); + GaussianFactorGraph A = createGaussianFactorGraph(ord); VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected; @@ -523,7 +523,7 @@ TEST(GaussianFactorGraph, hasConstraints) EXPECT(hasConstraints(fgc2)); Ordering ordering; ordering += X(1), X(2), L(1); - FactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(!hasConstraints(fg)); } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index ea91eda5a..74417b57a 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -42,7 +42,7 @@ using symbol_shorthand::L; // Some numbers that should be consistent among all smoother tests static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = - 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; + 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1; static const double tol = 1e-4; diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 09047005e..bfa3a8e79 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -16,13 +16,14 @@ **/ #include +#include +#include +#include #include #include #include -//#include -//#include -#include -//#include +#include +#include #include @@ -32,23 +33,23 @@ using namespace example; using symbol_shorthand::X; // to create pose keys using symbol_shorthand::L; // to create landmark keys -static bool verbose = false; +static ConjugateGradientParameters parameters; +// add following below to add printing: +// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY; /* ************************************************************************* */ TEST( Iterative, steepestDescent ) { // Create factor graph - Ordering ord; - ord += L(1), X(1), X(2); - FactorGraph fg = createGaussianFactorGraph(ord); + Ordering ordering; + ordering += L(1), X(1), X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // eliminate and solve VectorValues expected = *GaussianSequentialSolver(fg).optimize(); // Do gradient descent VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally? - ConjugateGradientParameters parameters; -// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY; VectorValues actual = steepestDescent(fg, zero, parameters); CHECK(assert_equal(expected,actual,1e-2)); } @@ -56,134 +57,93 @@ TEST( Iterative, steepestDescent ) /* ************************************************************************* */ TEST( Iterative, conjugateGradientDescent ) { -// // Expected solution -// Ordering ord; -// ord += L(1), X(1), X(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// VectorValues expected = fg.optimize(ord); // destructive -// -// // create graph and get matrices -// GaussianFactorGraph fg2 = createGaussianFactorGraph(); -// Matrix A; -// Vector b; -// Vector x0 = gtsam::zero(6); -// boost::tie(A, b) = fg2.matrix(ord); -// Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); -// -// // Do conjugate gradient descent, System version -// System Ab(A, b); -// Vector actualX = conjugateGradientDescent(Ab, x0, verbose); -// CHECK(assert_equal(expectedX,actualX,1e-9)); -// -// // Do conjugate gradient descent, Matrix version -// Vector actualX2 = conjugateGradientDescent(A, b, x0, verbose); -// CHECK(assert_equal(expectedX,actualX2,1e-9)); -// -// // Do conjugate gradient descent on factor graph -// VectorValues zero = createZeroDelta(); -// VectorValues actual = conjugateGradientDescent(fg2, zero, verbose); -// CHECK(assert_equal(expected,actual,1e-2)); -// -// // Test method -// VectorValues actual2 = fg2.conjugateGradientDescent(zero, verbose); -// CHECK(assert_equal(expected,actual2,1e-2)); + // Create factor graph + Ordering ordering; + ordering += L(1), X(1), X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + + // eliminate and solve + VectorValues expected = *GaussianSequentialSolver(fg).optimize(); + + // get matrices + Matrix A; + Vector b; + Vector x0 = gtsam::zero(6); + boost::tie(A, b) = fg.jacobian(); + Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); + + // Do conjugate gradient descent, System version + System Ab(A, b); + Vector actualX = conjugateGradientDescent(Ab, x0, parameters); + CHECK(assert_equal(expectedX,actualX,1e-9)); + + // Do conjugate gradient descent, Matrix version + Vector actualX2 = conjugateGradientDescent(A, b, x0, parameters); + CHECK(assert_equal(expectedX,actualX2,1e-9)); + + // Do conjugate gradient descent on factor graph + VectorValues zero = VectorValues::Zero(expected); + VectorValues actual = conjugateGradientDescent(fg, zero, parameters); + CHECK(assert_equal(expected,actual,1e-2)); } /* ************************************************************************* */ -/*TEST( Iterative, conjugateGradientDescent_hard_constraint ) +TEST( Iterative, conjugateGradientDescent_hard_constraint ) { - typedef Pose2Values::Key Key; + Values config; + Pose2 pose1 = Pose2(0.,0.,0.); + config.insert(X(1), pose1); + config.insert(X(2), Pose2(1.5,0.,0.)); - Pose2Values config; - config.insert(1, Pose2(0.,0.,0.)); - config.insert(2, Pose2(1.5,0.,0.)); + NonlinearFactorGraph graph; + graph.add(NonlinearEquality(X(1), pose1)); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); - Pose2Graph graph; - Matrix cov = eye(3); - graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov))); - graph.addHardConstraint(1, config[1]); + Ordering ordering; + ordering += X(1), X(2); + boost::shared_ptr fg = graph.linearize(config,ordering); - VectorValues zeros; - zeros.insert(X(1),zero(3)); - zeros.insert(X(2),zero(3)); + VectorValues zeros = VectorValues::Zero(2, 3); - GaussianFactorGraph fg = graph.linearize(config); - VectorValues actual = conjugateGradientDescent(fg, zeros, true, 1e-3, 1e-5, 10); + ConjugateGradientParameters parameters; + parameters.setEpsilon_abs(1e-3); + parameters.setEpsilon_rel(1e-5); + parameters.setMaxIterations(100); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); - VectorValues expected; - expected.insert(X(1), zero(3)); - expected.insert(X(2), Vector_(-0.5,0.,0.)); - CHECK(assert_equal(expected, actual)); -}*/ + VectorValues expected; + expected.insert(0, zero(3)); + expected.insert(1, Vector_(3,-0.5,0.,0.)); + CHECK(assert_equal(expected, actual)); +} /* ************************************************************************* */ TEST( Iterative, conjugateGradientDescent_soft_constraint ) { -// Pose2Values config; -// config.insert(1, Pose2(0.,0.,0.)); -// config.insert(2, Pose2(1.5,0.,0.)); -// -// Pose2Graph graph; -// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); -// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); -// -// VectorValues zeros; -// zeros.insert(X(1),zero(3)); -// zeros.insert(X(2),zero(3)); -// -// boost::shared_ptr fg = graph.linearize(config); -// VectorValues actual = conjugateGradientDescent(*fg, zeros, verbose, 1e-3, 1e-5, 100); -// -// VectorValues expected; -// expected.insert(X(1), zero(3)); -// expected.insert(X(2), Vector_(3,-0.5,0.,0.)); -// CHECK(assert_equal(expected, actual)); -} + Values config; + config.insert(X(1), Pose2(0.,0.,0.)); + config.insert(X(2), Pose2(1.5,0.,0.)); -/* ************************************************************************* */ -TEST( Iterative, subgraphPCG ) -{ -// typedef Pose2Values::Key Key; -// -// Pose2Values theta_bar; -// theta_bar.insert(1, Pose2(0.,0.,0.)); -// theta_bar.insert(2, Pose2(1.5,0.,0.)); -// -// Pose2Graph graph; -// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); -// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); -// -// // generate spanning tree and create ordering -// PredecessorMap tree = graph.findMinimumSpanningTree(); -// list keys = predecessorMap2Keys(tree); -// list symbols; -// symbols.resize(keys.size()); -// std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol); -// Ordering ordering(symbols); -// -// Key root = keys.back(); -// Pose2Graph T, C; -// graph.split(tree, T, C); -// -// // build the subgraph PCG system -// boost::shared_ptr Ab1_ = T.linearize(theta_bar); -// SubgraphPreconditioner::sharedFG Ab1 = T.linearize(theta_bar); -// SubgraphPreconditioner::sharedFG Ab2 = C.linearize(theta_bar); -// SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_->eliminate_(ordering); -// SubgraphPreconditioner::sharedValues xbar = optimize_(*Rc1); -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); -// -// VectorValues zeros = VectorValues::zero(*xbar); -// -// // Solve the subgraph PCG -// VectorValues ybar = conjugateGradients (system, zeros, verbose, 1e-5, 1e-5, 100); -// VectorValues actual = system.x(ybar); -// -// VectorValues expected; -// expected.insert(X(1), zero(3)); -// expected.insert(X(2), Vector_(3, -0.5, 0., 0.)); -// CHECK(assert_equal(expected, actual)); + NonlinearFactorGraph graph; + graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); + + Ordering ordering; + ordering += X(1), X(2); + boost::shared_ptr fg = graph.linearize(config,ordering); + + VectorValues zeros = VectorValues::Zero(2, 3); + + ConjugateGradientParameters parameters; + parameters.setEpsilon_abs(1e-3); + parameters.setEpsilon_rel(1e-5); + parameters.setMaxIterations(100); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); + + VectorValues expected; + expected.insert(0, zero(3)); + expected.insert(1, Vector_(3,-0.5,0.,0.)); + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */