diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 004303245..5d8d37c5b 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -178,6 +178,12 @@ namespace gtsam { bayesTree.addFactorsToGraph(*this); } + /** Add a factor or a shared_ptr to a factor, synonym for push_back() */ + template + void add(const FACTOR& factor) { + push_back(factor); + } + /** += syntax for push_back, e.g. graph += f1, f2, f3 */ //template //boost::assign::list_inserter > diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp index 38802f90c..5cbbd195c 100644 --- a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp @@ -78,7 +78,7 @@ namespace { * * 1 0 0 1 */ -TEST( GaussianBayesTreeOrdered, eliminate ) +TEST( GaussianBayesTree, eliminate ) { GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); @@ -94,7 +94,7 @@ TEST( GaussianBayesTreeOrdered, eliminate ) } /* ************************************************************************* */ -TEST( GaussianBayesTreeOrdered, optimizeMultiFrontal ) +TEST( GaussianBayesTree, optimizeMultiFrontal ) { VectorValues expected = pair_list_of (x1, Vector_(1, 0.)) @@ -107,7 +107,7 @@ TEST( GaussianBayesTreeOrdered, optimizeMultiFrontal ) } /* ************************************************************************* */ -TEST(GaussianBayesTreeOrdered, complicatedMarginal) { +TEST(GaussianBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree GaussianBayesTree bt; diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index 93a03447e..b4d51794e 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -397,7 +397,7 @@ TEST(JacobianFactor, eliminate2 ) } /* ************************************************************************* */ -TEST(JacobianFactor, EliminateQROrdered) +TEST(JacobianFactor, EliminateQR) { // Augmented Ab test case for whole factor graph Matrix Ab = Matrix_(14,11, diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 9c00da149..6c4617171 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -28,7 +28,7 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0); /* ************************************************************************* */ TEST( testLinearContainerFactor, generic_jacobian_factor ) { - OrderingOrdered initOrdering; initOrdering += x1, x2, l1, l2; + Ordering initOrdering; initOrdering += x1, x2, l1, l2; Matrix A1 = Matrix_(2,2, 2.74222, -0.0067457, @@ -39,7 +39,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { Vector b = Vector_(2, 0.0277052, -0.0533393); - JacobianFactorOrdered expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2); + JacobianFactor expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2); LinearContainerFactor actFactor(expLinFactor, initOrdering); EXPECT_LONGS_EQUAL(2, actFactor.size()); @@ -57,20 +57,20 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { values.insert(x2, poseA2); // Check reconstruction from same ordering - GaussianFactorOrdered::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering); + GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering); EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol)); // Check reconstruction from new ordering - OrderingOrdered newOrdering; newOrdering += x1, l1, x2, l2; - GaussianFactorOrdered::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering); - JacobianFactorOrdered expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2); + Ordering newOrdering; newOrdering += x1, l1, x2, l2; + GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering); + JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2); EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); } /* ************************************************************************* */ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { - OrderingOrdered ordering; ordering += x1, x2, l1, l2; + Ordering ordering; ordering += x1, x2, l1, l2; Matrix A1 = Matrix_(2,2, 2.74222, -0.0067457, @@ -81,7 +81,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { Vector b = Vector_(2, 0.0277052, -0.0533393); - JacobianFactorOrdered expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2); + JacobianFactor expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2); Values values; values.insert(l1, landmark1); @@ -108,7 +108,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { Vector delta_l1 = Vector_(2, 1.0, 2.0); Vector delta_l2 = Vector_(2, 3.0, 4.0); - VectorValuesOrdered delta = values.zeroVectors(ordering); + VectorValues delta = values.zeroVectors(ordering); delta.at(ordering[l1]) = delta_l1; delta.at(ordering[l2]) = delta_l2; Values noisyValues = values.retract(delta, ordering); @@ -117,10 +117,10 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol); // Check linearization with corrections for updated linearization point - OrderingOrdered newOrdering; newOrdering += x1, l1, x2, l2; - GaussianFactorOrdered::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering); + Ordering newOrdering; newOrdering += x1, l1, x2, l2; + GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering); Vector bprime = b - A1 * delta_l1 - A2 * delta_l2; - JacobianFactorOrdered expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2); + JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2); EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); } @@ -145,8 +145,8 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { double f = 10.0; - OrderingOrdered initOrdering; initOrdering += x1, x2, l1, l2; - HessianFactorOrdered initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1], + Ordering initOrdering; initOrdering += x1, x2, l1, l2; + HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1], G11, G12, G13, g1, G22, G23, g2, G33, g3, f); Values values; @@ -159,13 +159,13 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { EXPECT(!actFactor.isJacobian()); EXPECT(actFactor.isHessian()); - GaussianFactorOrdered::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering); + GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering); EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol)); - OrderingOrdered newOrdering; newOrdering += l1, x1, x2, l2; - HessianFactorOrdered expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1], + Ordering newOrdering; newOrdering += l1, x1, x2, l2; + HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1], G11, G12, G13, g1, G22, G23, g2, G33, g3, f); - GaussianFactorOrdered::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering); + GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering); EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol)); } @@ -196,8 +196,8 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { Matrix G(5,5); G << G11, G12, Matrix::Zero(2,3), G22; - OrderingOrdered ordering; ordering += x1, x2, l1; - HessianFactorOrdered initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f); + Ordering ordering; ordering += x1, x2, l1; + HessianFactor initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f); Values linearizationPoint, expLinPoints; linearizationPoint.insert(l1, landmark1); @@ -219,7 +219,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3); // Check error calculation - VectorValuesOrdered delta = linearizationPoint.zeroVectors(ordering); + VectorValues delta = linearizationPoint.zeroVectors(ordering); delta.at(ordering[l1]) = delta_l1; delta.at(ordering[x1]) = delta_x1; delta.at(ordering[x2]) = delta_x2; @@ -239,7 +239,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { Vector g1_prime = g_prime.head(3); Vector g2_prime = g_prime.tail(2); double f_prime = f + dv.transpose() * G.selfadjointView() * dv - 2.0 * dv.transpose() * g; - HessianFactorOrdered expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime); + HessianFactor expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime); EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues, ordering), tol)); } @@ -252,12 +252,12 @@ TEST( testLinearContainerFactor, creation ) { l7 = 17, l8 = 18; // creating an ordering to decode the linearized factor - OrderingOrdered ordering; + Ordering ordering; ordering += l1,l2,l3,l4,l5,l6,l7,l8; // create a linear factor SharedDiagonal model = noiseModel::Unit::Create(2); - JacobianFactorOrdered::shared_ptr linear_factor(new JacobianFactorOrdered( + JacobianFactor::shared_ptr linear_factor(new JacobianFactor( ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model)); // create a set of values - build with full set of values @@ -284,7 +284,7 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) // Create a Between Factor from a Point3. This is actually a linear factor. gtsam::Key key1(1); gtsam::Key key2(2); - gtsam::OrderingOrdered ordering; + gtsam::Ordering ordering; ordering.push_back(key1); ordering.push_back(key2); gtsam::Values linpoint1; @@ -296,7 +296,7 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) gtsam::BetweenFactor betweenFactor(key1, key2, measured, model); // Create a jacobian container factor at linpoint 1 - gtsam::JacobianFactorOrdered::shared_ptr jacobian(new gtsam::JacobianFactorOrdered(*betweenFactor.linearize(linpoint1, ordering))); + gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1, ordering))); gtsam::LinearContainerFactor jacobianContainer(jacobian, ordering, linpoint1); // Create a second linearization point @@ -310,8 +310,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // Re-linearize around the new point and check the factors - gtsam::GaussianFactorOrdered::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering); - gtsam::GaussianFactorOrdered::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering); + gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering); + gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); } @@ -321,7 +321,7 @@ TEST( testLinearContainerFactor, hessian_relinearize ) // Create a Between Factor from a Point3. This is actually a linear factor. gtsam::Key key1(1); gtsam::Key key2(2); - gtsam::OrderingOrdered ordering; + gtsam::Ordering ordering; ordering.push_back(key1); ordering.push_back(key2); gtsam::Values linpoint1; @@ -333,7 +333,7 @@ TEST( testLinearContainerFactor, hessian_relinearize ) gtsam::BetweenFactor betweenFactor(key1, key2, measured, model); // Create a hessian container factor at linpoint 1 - gtsam::HessianFactorOrdered::shared_ptr hessian(new gtsam::HessianFactorOrdered(*betweenFactor.linearize(linpoint1, ordering))); + gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1, ordering))); gtsam::LinearContainerFactor hessianContainer(hessian, ordering, linpoint1); // Create a second linearization point @@ -347,8 +347,8 @@ TEST( testLinearContainerFactor, hessian_relinearize ) EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // Re-linearize around the new point and check the factors - gtsam::GaussianFactorOrdered::shared_ptr expected_factor = gtsam::HessianFactorOrdered::shared_ptr(new gtsam::HessianFactorOrdered(*betweenFactor.linearize(linpoint2, ordering))); - gtsam::GaussianFactorOrdered::shared_ptr actual_factor = hessianContainer.linearize(linpoint2, ordering); + gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2, ordering))); + gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2, ordering); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index de92103a3..cb4973ef5 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -24,6 +25,7 @@ #include #include // for operator += +#include using namespace boost::assign; #include #include @@ -155,7 +157,7 @@ TEST( Values, update_element ) // config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // -// VectorValuesOrdered expected; +// VectorValues expected; // expected.insert(key1, zero(2)); // expected.insert(key2, zero(3)); // CHECK(assert_equal(expected, config0.zero())); @@ -168,16 +170,15 @@ TEST(Values, expmap_a) config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); - OrderingOrdered ordering(*config0.orderingArbitrary()); - VectorValuesOrdered increment(config0.dims(ordering)); - increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); - increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); + VectorValues increment = pair_list_of + (key1, Vector_(3, 1.0, 1.1, 1.2)) + (key2, Vector_(3, 1.3, 1.4, 1.5)); Values expected; expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); - CHECK(assert_equal(expected, config0.retract(increment, ordering))); + CHECK(assert_equal(expected, config0.retract(increment))); } /* ************************************************************************* */ @@ -187,15 +188,14 @@ TEST(Values, expmap_b) config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); - OrderingOrdered ordering(*config0.orderingArbitrary()); - VectorValuesOrdered increment(VectorValuesOrdered::Zero(config0.dims(ordering))); - increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5); + VectorValues increment = pair_list_of + (key2, LieVector(3, 1.3, 1.4, 1.5)); Values expected; expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); - CHECK(assert_equal(expected, config0.retract(increment, ordering))); + CHECK(assert_equal(expected, config0.retract(increment))); } /* ************************************************************************* */ @@ -241,15 +241,13 @@ TEST(Values, localCoordinates) valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); - OrderingOrdered ordering = *valuesA.orderingArbitrary(); + VectorValues expDelta = pair_list_of + (key1, Vector_(3, 0.1, 0.2, 0.3)) + (key2, Vector_(3, 0.4, 0.5, 0.6)); - VectorValuesOrdered expDelta = valuesA.zeroVectors(ordering); -// expDelta.at(ordering[key1]) = Vector_(3, 0.1, 0.2, 0.3); -// expDelta.at(ordering[key2]) = Vector_(3, 0.4, 0.5, 0.6); + Values valuesB = valuesA.retract(expDelta); - Values valuesB = valuesA.retract(expDelta, ordering); - - EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB, ordering))); + EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB))); } /* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp index ac13d2374..332b91ce7 100644 --- a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp +++ b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp @@ -10,7 +10,6 @@ #include #include -#include #include @@ -31,7 +30,7 @@ static const double tol = 1e-4; TEST( testBayesTreeOperations, splitFactor1 ) { // Build upper-triangular system - JacobianFactorOrdered initFactor( + JacobianFactor initFactor( 0,Matrix_(4, 2, 1.0, 2.0, 0.0, 3.0, @@ -45,8 +44,8 @@ TEST( testBayesTreeOperations, splitFactor1 ) { Vector_(4, 0.1, 0.2, 0.3, 0.4), model4); - GaussianFactorGraphOrdered actSplit = splitFactor(initFactor.clone()); - GaussianFactorGraphOrdered expSplit; + GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); + GaussianFactorGraph expSplit; expSplit.add( 0,Matrix_(2, 2, @@ -71,7 +70,7 @@ TEST( testBayesTreeOperations, splitFactor1 ) { TEST( testBayesTreeOperations, splitFactor2 ) { // Build upper-triangular system - JacobianFactorOrdered initFactor( + JacobianFactor initFactor( 0,Matrix_(6, 2, 1.0, 2.0, 0.0, 3.0, @@ -96,8 +95,8 @@ TEST( testBayesTreeOperations, splitFactor2 ) { Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6), model6); - GaussianFactorGraphOrdered actSplit = splitFactor(initFactor.clone()); - GaussianFactorGraphOrdered expSplit; + GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); + GaussianFactorGraph expSplit; expSplit.add( 0,Matrix_(2, 2, @@ -134,7 +133,7 @@ TEST( testBayesTreeOperations, splitFactor2 ) { TEST( testBayesTreeOperations, splitFactor3 ) { // Build upper-triangular system - JacobianFactorOrdered initFactor( + JacobianFactor initFactor( 0,Matrix_(4, 2, 1.0, 2.0, 0.0, 3.0, @@ -153,8 +152,8 @@ TEST( testBayesTreeOperations, splitFactor3 ) { Vector_(4, 0.1, 0.2, 0.3, 0.4), model4); - GaussianFactorGraphOrdered actSplit = splitFactor(initFactor.clone()); - GaussianFactorGraphOrdered expSplit; + GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); + GaussianFactorGraph expSplit; expSplit.add( 0,Matrix_(2, 2, @@ -192,12 +191,12 @@ TEST( testBayesTreeOperations, liquefy ) { using namespace example; // Create smoother with 7 nodes - OrderingOrdered ordering; + Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree - GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); // bayesTree.print("Full tree"); SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6))); @@ -206,8 +205,8 @@ TEST( testBayesTreeOperations, liquefy ) { // Liquefy the tree back into a graph { - GaussianFactorGraphOrdered actGraph = liquefy(bayesTree, false); // Doesn't split conditionals - GaussianFactorGraphOrdered expGraph; + GaussianFactorGraph actGraph = liquefy(bayesTree, false); // Doesn't split conditionals + GaussianFactorGraph expGraph; Matrix A12 = Matrix_(6, 2, 1.73205081, 0.0, @@ -278,8 +277,8 @@ TEST( testBayesTreeOperations, liquefy ) { // Liquefy the tree back into a graph, splitting factors { - GaussianFactorGraphOrdered actGraph = liquefy(bayesTree, true); - GaussianFactorGraphOrdered expGraph; + GaussianFactorGraph actGraph = liquefy(bayesTree, true); + GaussianFactorGraph expGraph; // Conditional 1 { diff --git a/gtsam_unstable/linear/tests/testConditioning.cpp b/gtsam_unstable/linear/tests/testConditioning.cpp index d01e4ff4d..69646fbdc 100644 --- a/gtsam_unstable/linear/tests/testConditioning.cpp +++ b/gtsam_unstable/linear/tests/testConditioning.cpp @@ -65,10 +65,10 @@ TEST( testConditioning, directed_elimination_singlefrontal ) { Index root_key = 0, removed_key = 1, remaining_parent = 2; Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0); Vector d0 = d.segment(0,1), d1 = d.segment(1,1), sigmas = Vector_(1, 1.0); - GaussianConditionalOrdered::shared_ptr initConditional(new - GaussianConditionalOrdered(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas)); + GaussianConditional::shared_ptr initConditional(new + GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas)); - VectorValuesOrdered solution; + VectorValues solution; solution.insert(0, x.segment(0,1)); solution.insert(1, x.segment(1,1)); solution.insert(2, x.segment(2,1)); @@ -76,22 +76,22 @@ TEST( testConditioning, directed_elimination_singlefrontal ) { std::set saved_indices; saved_indices += root_key, remaining_parent; - GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - GaussianConditionalOrdered::shared_ptr expSummarized(new - GaussianConditionalOrdered(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas)); + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + GaussianConditional::shared_ptr expSummarized(new + GaussianConditional(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas)); CHECK(actSummarized); EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); // Simple test of base case: if target index isn't present, return clone - GaussianConditionalOrdered::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution); + GaussianConditional::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution); CHECK(actSummarizedSimple); EXPECT(assert_equal(*expSummarized, *actSummarizedSimple, tol)); // case where frontal variable is to be eliminated - return null - GaussianConditionalOrdered::shared_ptr removeFrontalInit(new - GaussianConditionalOrdered(removed_key, d1, R22, remaining_parent, T, sigmas)); - GaussianConditionalOrdered::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution); + GaussianConditional::shared_ptr removeFrontalInit(new + GaussianConditional(removed_key, d1, R22, remaining_parent, T, sigmas)); + GaussianConditional::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution); EXPECT(!actRemoveFrontal); } @@ -108,9 +108,9 @@ TEST( testConditioning, directed_elimination_multifrontal ) { terms += make_pair(root_key, Matrix(R11.col(0))); terms += make_pair(removed_key, Matrix(R11.col(1))); terms += make_pair(remaining_parent, S); - GaussianConditionalOrdered::shared_ptr initConditional(new GaussianConditionalOrdered(terms, 2, d1, sigmas2)); + GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2)); - VectorValuesOrdered solution; + VectorValues solution; solution.insert(0, x.segment(0,1)); solution.insert(1, x.segment(1,1)); solution.insert(2, x.segment(2,1)); @@ -118,9 +118,9 @@ TEST( testConditioning, directed_elimination_multifrontal ) { std::set saved_indices; saved_indices += root_key, remaining_parent; - GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - GaussianConditionalOrdered::shared_ptr expSummarized(new - GaussianConditionalOrdered(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1)); + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + GaussianConditional::shared_ptr expSummarized(new + GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1)); CHECK(actSummarized); EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); @@ -140,14 +140,14 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim ) { 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6); vector init_dims; init_dims += 2, 2, 2, 2, 2, 1; - GaussianConditionalOrdered::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); + GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); Vector sigmas = ones(6); vector init_keys; init_keys += 0, 1, 2, 3, 4; - GaussianConditionalOrdered::shared_ptr initConditional(new - GaussianConditionalOrdered(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas)); + GaussianConditional::shared_ptr initConditional(new + GaussianConditional(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas)); // Construct a solution vector - VectorValuesOrdered solution; + VectorValues solution; solution.insert(0, zero(2)); solution.insert(1, zero(2)); solution.insert(2, zero(2)); @@ -159,7 +159,7 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim ) { std::set saved_indices; saved_indices += 0, 2, 4; - GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); CHECK(actSummarized); Matrix Rexp = Matrix_(4, 7, @@ -173,10 +173,10 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim ) { Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3); vector exp_dims; exp_dims += 2, 2, 2, 1; - GaussianConditionalOrdered::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); + GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); Vector exp_sigmas = ones(4); vector exp_keys; exp_keys += 0, 2, 4; - GaussianConditionalOrdered expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas); + GaussianConditional expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas); EXPECT(assert_equal(expSummarized, *actSummarized, tol)); } @@ -205,13 +205,13 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) { Rinit.rightCols(1) = dinit; Vector sigmas = ones(10); - GaussianConditionalOrdered::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); + GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); vector init_keys; init_keys += 3, 4, 5, 6; - GaussianConditionalOrdered::shared_ptr initConditional(new - GaussianConditionalOrdered(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas)); + GaussianConditional::shared_ptr initConditional(new + GaussianConditional(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas)); // Calculate a solution - VectorValuesOrdered solution; + VectorValues solution; solution.insert(0, zero(3)); solution.insert(1, zero(3)); solution.insert(2, zero(3)); @@ -226,7 +226,7 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) { std::set saved_indices; saved_indices += 5, 6; - GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); CHECK(actSummarized); // Create expected value on [5], [6] @@ -238,9 +238,9 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) { Vector expsigmas = ones(4); vector exp_dims; exp_dims += 2, 2, 1; - GaussianConditionalOrdered::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); + GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); vector exp_keys; exp_keys += 5, 6; - GaussianConditionalOrdered expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas); + GaussianConditional expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas); EXPECT(assert_equal(expConditional, *actSummarized, tol)); } diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 53828d6b4..f4b326719 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -20,10 +20,10 @@ #include #include #include +#include #include -#include -#include -#include +#include +#include #include #include #include @@ -35,11 +35,9 @@ using namespace gtsam; /* ************************************************************************* */ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const BatchFixedLagSmoother& smoother, const Key& key) { - OrderingOrdered ordering = *fullgraph.orderingCOLAMD(fullinit); - GaussianFactorGraphOrdered linearized = *fullgraph.linearize(fullinit, ordering); - GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValuesOrdered delta = optimize(gbn); - Values fullfinal = fullinit.retract(delta, ordering); + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit); + VectorValues delta = linearized.optimize(); + Values fullfinal = fullinit.retract(delta); Point2 expected = fullfinal.at(key); Point2 actual = smoother.calculateEstimate(key); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index ac4a6a390..f4f4190e9 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -23,12 +23,12 @@ #include #include #include -#include #include #include +#include +#include #include -#include -#include +#include #include #include @@ -86,22 +86,16 @@ bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGr return false; } - // Create an ordering - OrderingOrdered ordering; - BOOST_FOREACH(Key key, expectedKeys) { - ordering.push_back(key); - } - // Linearize each factor graph - GaussianFactorGraphOrdered expectedGaussian; + GaussianFactorGraph expectedGaussian; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) { if(factor) - expectedGaussian.push_back( factor->linearize(theta, ordering) ); + expectedGaussian.push_back( factor->linearize(theta) ); } - GaussianFactorGraphOrdered actualGaussian; + GaussianFactorGraph actualGaussian; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) { if(factor) - actualGaussian.push_back( factor->linearize(theta, ordering) ); + actualGaussian.push_back( factor->linearize(theta) ); } // Convert linear factor graph into a dense Hessian @@ -384,7 +378,7 @@ NonlinearFactorGraph MarginalFactors(const NonlinearFactorGraph& factors, const BOOST_FOREACH(Key key, remainingKeys) { constraints[key] = 1; } - OrderingOrdered ordering = *factors.orderingCOLAMDConstrained(values, constraints); + Ordering ordering = *factors.orderingCOLAMDConstrained(values, constraints); // Convert the remaining keys into indices std::vector remainingIndices; @@ -394,7 +388,7 @@ NonlinearFactorGraph MarginalFactors(const NonlinearFactorGraph& factors, const // Solve for the Gaussian marginal factors GaussianSequentialSolver gss(*factors.linearize(values, ordering), true); - GaussianFactorGraphOrdered linearMarginals = *gss.jointFactorGraph(remainingIndices); + GaussianFactorGraph linearMarginals = *gss.jointFactorGraph(remainingIndices); // Convert to LinearContainFactors return LinearContainerFactor::convertLinearGraph(linearMarginals, ordering, values); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 99f7e6adb..04b6f4788 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -23,11 +23,11 @@ #include #include #include -#include #include #include +#include #include -#include +#include #include #include @@ -78,18 +78,18 @@ bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGr return false; // Create an ordering - OrderingOrdered ordering; + Ordering ordering; BOOST_FOREACH(Key key, expectedKeys) { ordering.push_back(key); } // Linearize each factor graph - GaussianFactorGraphOrdered expectedGaussian; + GaussianFactorGraph expectedGaussian; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) { if(factor) expectedGaussian.push_back( factor->linearize(theta, ordering) ); } - GaussianFactorGraphOrdered actualGaussian; + GaussianFactorGraph actualGaussian; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) { if(factor) actualGaussian.push_back( factor->linearize(theta, ordering) ); @@ -511,7 +511,7 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) Values optimalTheta = BatchOptimize(fullGraph, fullTheta); // Re-eliminate to create the Bayes Tree - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(Symbol('X', 2)); ordering.push_back(Symbol('X', 0)); ordering.push_back(Symbol('X', 1)); @@ -527,10 +527,10 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) ordering.push_back(Symbol('X', 12)); Values linpoint; linpoint.insert(optimalTheta); - GaussianFactorGraphOrdered linearGraph = *fullGraph.linearize(linpoint, ordering); - JunctionTreeOrdered jt(linearGraph); - ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQROrdered); - BayesTreeOrdered bayesTree; + GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering); + JunctionTree jt(linearGraph); + ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); + BayesTree bayesTree; bayesTree.insert(root); // Extract the values for the smoother keys. This consists of the branches: X4 and X6 @@ -637,9 +637,9 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) linpoint.insert(optimalTheta); linpoint.update(rootValues); linearGraph = *fullGraph.linearize(linpoint, ordering); - jt = JunctionTreeOrdered(linearGraph); - root = jt.eliminate(EliminateQROrdered); - bayesTree = BayesTreeOrdered(); + jt = JunctionTree(linearGraph); + root = jt.eliminate(EliminateQR); + bayesTree = BayesTree(); bayesTree.insert(root); // Add the loop closure to the smoother diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 19aecc7f2..0a4a1fa74 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -22,11 +22,11 @@ #include #include #include -#include -#include +#include +#include +#include #include #include -#include #include #include #include @@ -39,11 +39,9 @@ Key MakeKey(size_t index) { return Symbol('x', index); } /* ************************************************************************* */ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) { - OrderingOrdered ordering = *fullgraph.orderingCOLAMD(fullinit); - GaussianFactorGraphOrdered linearized = *fullgraph.linearize(fullinit, ordering); - GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValuesOrdered delta = optimize(gbn); - Values fullfinal = fullinit.retract(delta, ordering); + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit); + VectorValues delta = linearized.optimize(); + Values fullfinal = fullinit.retract(delta); Point2 expected = fullfinal.at(key); Point2 actual = smoother.calculateEstimate(key);