diff --git a/gtsam/base/tests/testKruskal.cpp b/gtsam/base/tests/testKruskal.cpp index 9d808670a..bb8cfcaca 100644 --- a/gtsam/base/tests/testKruskal.cpp +++ b/gtsam/base/tests/testKruskal.cpp @@ -37,12 +37,12 @@ gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() { Matrix I = I_2x2; Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); - gfg += JacobianFactor(X(1), I, X(2), I, b, model); - gfg += JacobianFactor(X(1), I, X(3), I, b, model); - gfg += JacobianFactor(X(1), I, X(4), I, b, model); - gfg += JacobianFactor(X(2), I, X(3), I, b, model); - gfg += JacobianFactor(X(2), I, X(4), I, b, model); - gfg += JacobianFactor(X(3), I, X(4), I, b, model); + gfg.emplace_shared(X(1), I, X(2), I, b, model); + gfg.emplace_shared(X(1), I, X(3), I, b, model); + gfg.emplace_shared(X(1), I, X(4), I, b, model); + gfg.emplace_shared(X(2), I, X(3), I, b, model); + gfg.emplace_shared(X(2), I, X(4), I, b, model); + gfg.emplace_shared(X(3), I, X(4), I, b, model); return gfg; } @@ -53,12 +53,12 @@ gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() { NonlinearFactorGraph nfg; const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); - nfg += BetweenFactor(X(1), X(2), Rot3(), model); - nfg += BetweenFactor(X(1), X(3), Rot3(), model); - nfg += BetweenFactor(X(1), X(4), Rot3(), model); - nfg += BetweenFactor(X(2), X(3), Rot3(), model); - nfg += BetweenFactor(X(2), X(4), Rot3(), model); - nfg += BetweenFactor(X(3), X(4), Rot3(), model); + nfg.emplace_shared>(X(1), X(2), Rot3(), model); + nfg.emplace_shared>(X(1), X(3), Rot3(), model); + nfg.emplace_shared>(X(1), X(4), Rot3(), model); + nfg.emplace_shared>(X(2), X(3), Rot3(), model); + nfg.emplace_shared>(X(2), X(4), Rot3(), model); + nfg.emplace_shared>(X(3), X(4), Rot3(), model); return nfg; } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index f52d69e47..966b70915 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -277,15 +277,15 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) TEST( GaussianBayesNet, DeterminantTest ) { GaussianBayesNet cbn; - cbn += GaussianConditional( + cbn.emplace_shared( 0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(), 1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); - cbn += GaussianConditional( + cbn.emplace_shared( 1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(), 2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); - cbn += GaussianConditional( + cbn.emplace_shared( 3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); double expectedDeterminant = 60.0 / 64.0; @@ -308,22 +308,22 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + gbn.emplace_shared( 0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(), - 4, (Matrix2() << 11.0,12.0,13.0,14.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix2() << 11.0,12.0,13.0,14.0).finished()); + gbn.emplace_shared( 1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(), - 4, (Matrix2() << 25.0,26.0,27.0,28.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix2() << 25.0,26.0,27.0,28.0).finished()); + gbn.emplace_shared( 2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(), - 3, (Matrix2() << 35.0,36.0,37.0,38.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, (Matrix2() << 35.0,36.0,37.0,38.0).finished()); + gbn.emplace_shared( 3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(), - 4, (Matrix2() << 45.0,46.0,47.0,48.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished())); + 4, (Matrix2() << 45.0,46.0,47.0,48.0).finished()); + gbn.emplace_shared( + 4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished()); // Compute the Hessian numerically Matrix hessian = numericalHessian( diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 52a3123bf..899d69814 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -299,10 +299,10 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { GaussianFactorGraph fg; Key x1 = 2, x2 = 0, l1 = 1; SharedDiagonal unit2 = noiseModel::Unit::Create(2); - fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); - fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); - fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); - fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + fg.emplace_shared(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + fg.emplace_shared(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); // create corresponding Bayes tree: std::shared_ptr bt = fg.eliminateMultifrontal(); @@ -327,15 +327,14 @@ TEST(GaussianBayesTree, LogDeterminant) { GaussianFactorGraph fg; Key x1 = X(1), x2 = X(2), l1 = L(1); SharedDiagonal unit2 = noiseModel::Unit::Create(2); - fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); - fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), - unit2); - fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); - fg += - JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); - fg += JacobianFactor(x3, 10 * I_2x2, x2, -10 * I_2x2, Vector2(2.0, -1.0), - unit2); - fg += JacobianFactor(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + fg.emplace_shared(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + fg.emplace_shared(x2, 10 * I_2x2, x1, -10 * I_2x2, + Vector2(2.0, -1.0), unit2); + fg.emplace_shared(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(x3, 10 * I_2x2, x2, -10 * I_2x2, + Vector2(2.0, -1.0), unit2); + fg.emplace_shared(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); // create corresponding Bayes net and Bayes tree: std::shared_ptr bn = fg.eliminateSequential(); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index bf8127541..e9e626296 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -51,11 +51,10 @@ TEST(GaussianFactorGraph, initialization) { GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); - fg += - JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2), - JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2), - JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2), - JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2); + fg.emplace_shared(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2); + fg.emplace_shared(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); @@ -186,13 +185,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { Key x1 = 2, x2 = 0, l1 = 1; SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + fg.emplace_shared(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + fg.emplace_shared(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); return fg; } diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp index 2e4d2249e..b86451600 100644 --- a/gtsam/linear/tests/testRegularHessianFactor.cpp +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -66,11 +66,11 @@ TEST(RegularHessianFactor, Constructors) // Test constructor from Gaussian Factor Graph GaussianFactorGraph gfg; - gfg += jf; + gfg.push_back(jf); RegularHessianFactor<2> factor4(gfg); EXPECT(assert_equal(factor, factor4)); GaussianFactorGraph gfg2; - gfg2 += factor; + gfg2.push_back(factor); RegularHessianFactor<2> factor5(gfg); EXPECT(assert_equal(factor, factor5)); diff --git a/tests/smallExample.h b/tests/smallExample.h index 5ffb5d47b..c5ab10594 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -274,16 +274,16 @@ inline GaussianFactorGraph createGaussianFactorGraph() { GaussianFactorGraph fg; // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Vector::Ones(2)); + fg.emplace_shared(X(1), 10*I_2x2, -1.0*Vector::Ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); + fg.emplace_shared(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0)); + fg.emplace_shared(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5)); + fg.emplace_shared(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5)); return fg; } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index c67914c8b..4aa357ba2 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -144,9 +144,9 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { NonlinearFactorGraph graph; Symbol x1('x',1); - graph += iq2D::PoseXInequality(x1, 1.0, true); - graph += iq2D::PoseYInequality(x1, 2.0, true); - graph += simulated2D::Prior(start_pt, soft_model2, x1); + graph.emplace_shared(x1, 1.0, true); + graph.emplace_shared(x1, 2.0, true); + graph.emplace_shared(start_pt, soft_model2, x1); Values initValues; initValues.insert(x1, start_pt); @@ -165,9 +165,9 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Point2 start_pt(2.0, 3.0); NonlinearFactorGraph graph; - graph += iq2D::PoseXInequality(key, 1.0, false); - graph += iq2D::PoseYInequality(key, 2.0, false); - graph += simulated2D::Prior(start_pt, soft_model2, key); + graph.emplace_shared(key, 1.0, false); + graph.emplace_shared(key, 2.0, false); + graph.emplace_shared(start_pt, soft_model2, key); Values initValues; initValues.insert(key, start_pt); @@ -224,9 +224,9 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { Symbol x1('x',1), x2('x',2); NonlinearFactorGraph graph; - graph += simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1); - graph += simulated2D::Prior(pt2_init, soft_model2_alt, x2); - graph += iq2D::PoseMaxDistConstraint(x1, x2, 2.0); + graph.emplace_shared(pt1, x1); + graph.emplace_shared(pt2_init, soft_model2_alt, x2); + graph.emplace_shared(x1, x2, 2.0); Values initial_state; initial_state.insert(x1, pt1); @@ -250,12 +250,12 @@ TEST( testBoundingConstraint, avoid_demo) { Point2 odo(2.0, 0.0); NonlinearFactorGraph graph; - graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1); - graph += simulated2D::Odometry(odo, soft_model2_alt, x1, x2); - graph += iq2D::LandmarkAvoid(x2, l1, radius); - graph += simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1); - graph += simulated2D::Odometry(odo, soft_model2_alt, x2, x3); - graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3); + graph.emplace_shared(x1_pt, x1); + graph.emplace_shared(odo, soft_model2_alt, x1, x2); + graph.emplace_shared(x2, l1, radius); + graph.emplace_shared(l1_pt, l1); + graph.emplace_shared(odo, soft_model2_alt, x2, x3); + graph.emplace_shared(x3_pt, x3); Values init, expected; init.insert(x1, x1_pt); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index a64e6f3b0..e64e5ab7a 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -42,22 +42,22 @@ using symbol_shorthand::L; TEST(DoglegOptimizer, ComputeBlend) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + gbn.emplace_shared( 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), - 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()); + gbn.emplace_shared( 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), - 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()); + gbn.emplace_shared( 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), - 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()); + gbn.emplace_shared( 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), - 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()); + gbn.emplace_shared( + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()); // Compute steepest descent point VectorValues xu = gbn.optimizeGradientSearch(); @@ -78,22 +78,22 @@ TEST(DoglegOptimizer, ComputeBlend) { TEST(DoglegOptimizer, ComputeDoglegPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + gbn.emplace_shared( 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), - 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()); + gbn.emplace_shared( 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), - 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()); + gbn.emplace_shared( 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), - 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()); + gbn.emplace_shared( 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), - 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()); + gbn.emplace_shared( + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()); // Compute dogleg point for different deltas diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 1a2e89dcb..bb1180903 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -75,7 +75,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); GaussianBayesNet expected3; - expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3); + expected3.emplace_shared(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -84,7 +84,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); GaussianBayesNet expected4; - expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4); + expected4.emplace_shared(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 4326e94df..36f618c59 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -427,10 +427,10 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { 0.0, 1., 0.0, -1.2246468e-16, 0.0, -1), Point3(0.511832102, 8.42819594, 5.76841725)), priorModel); - factors += ProjectionFactor(Point2(333.648615, 98.61535), measModel, xC1, l32, K); - factors += ProjectionFactor(Point2(218.508, 83.8022039), measModel, xC1, l41, K); - factors += RangeFactor(xC1, l32, relElevation, elevationModel); - factors += RangeFactor(xC1, l41, relElevation, elevationModel); + factors.emplace_shared(Point2(333.648615, 98.61535), measModel, xC1, l32, K); + factors.emplace_shared(Point2(218.508, 83.8022039), measModel, xC1, l41, K); + factors.emplace_shared>(xC1, l32, relElevation, elevationModel); + factors.emplace_shared>(xC1, l41, relElevation, elevationModel); // Check that sigmas are correct (i.e., unit) GaussianFactorGraph lfg = *factors.linearize(initValues); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 23b3609f3..5331b7dc4 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -74,7 +74,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 0 to time 5 for( ; i<5; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -93,9 +93,9 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 5 to 6 and landmark measurement at time 5 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); Values init; @@ -116,7 +116,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 6 to time 10 for( ; i<10; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -135,9 +135,9 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 10 to 11 and landmark measurement at time 10 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); Values init; @@ -230,7 +230,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c // Check information GaussianFactorGraph isamGraph(isam); - isamGraph += isam.roots().front()->cachedFactor_; + isamGraph.push_back(isam.roots().front()->cachedFactor_); Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian(); Matrix actualHessian = isamGraph.augmentedHessian(); expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1); @@ -249,7 +249,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c for (const auto& [key, clique] : isam.nodes()) { // Compute expected gradient GaussianFactorGraph jfg; - jfg += clique->conditional(); + jfg.push_back(clique->conditional()); VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients DenseIndex variablePosition = 0; @@ -353,7 +353,7 @@ TEST(ISAM2, clone) { // Modify original isam NonlinearFactorGraph factors; - factors += BetweenFactor(0, 10, + factors.emplace_shared>(0, 10, isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3)); isam.update(factors); @@ -439,7 +439,7 @@ TEST(ISAM2, swapFactors) NonlinearFactorGraph swapfactors; // swapfactors += BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor - swapfactors += BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); + swapfactors.emplace_shared>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); fullgraph.push_back(swapfactors); isam.update(swapfactors, Values(), toRemove); } @@ -452,7 +452,7 @@ TEST(ISAM2, swapFactors) for (const auto& [key, clique]: isam.nodes()) { // Compute expected gradient GaussianFactorGraph jfg; - jfg += clique->conditional(); + jfg.push_back(clique->conditional()); VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients DenseIndex variablePosition = 0; @@ -507,7 +507,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -523,9 +523,9 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 5 to 6 and landmark measurement at time 5 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); Values init; @@ -543,7 +543,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -556,9 +556,9 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 10 to 11 and landmark measurement at time 10 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); Values init; @@ -576,7 +576,7 @@ TEST(ISAM2, constrained_ordering) for (const auto& [key, clique]: isam.nodes()) { // Compute expected gradient GaussianFactorGraph jfg; - jfg += clique->conditional(); + jfg.push_back(clique->conditional()); VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients DenseIndex variablePosition = 0; @@ -665,9 +665,9 @@ TEST(ISAM2, marginalizeLeaves1) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 1, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); + factors.emplace_shared>(0, 1, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); Values values; values.insert(0, 0.0); @@ -692,10 +692,10 @@ TEST(ISAM2, marginalizeLeaves2) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 1, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); - factors += BetweenFactor(2, 3, 0.0, model); + factors.emplace_shared>(0, 1, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); + factors.emplace_shared>(2, 3, 0.0, model); Values values; values.insert(0, 0.0); @@ -722,15 +722,15 @@ TEST(ISAM2, marginalizeLeaves3) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 1, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); + factors.emplace_shared>(0, 1, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); - factors += BetweenFactor(2, 3, 0.0, model); + factors.emplace_shared>(2, 3, 0.0, model); - factors += BetweenFactor(3, 4, 0.0, model); - factors += BetweenFactor(4, 5, 0.0, model); - factors += BetweenFactor(3, 5, 0.0, model); + factors.emplace_shared>(3, 4, 0.0, model); + factors.emplace_shared>(4, 5, 0.0, model); + factors.emplace_shared>(3, 5, 0.0, model); Values values; values.insert(0, 0.0); @@ -760,8 +760,8 @@ TEST(ISAM2, marginalizeLeaves4) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); Values values; values.insert(0, 0.0); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 68508e6df..d7ca70459 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -39,15 +39,15 @@ std::tuple generateProblem() { // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); - graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); + graph.emplace_shared>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas( Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), + graph.emplace_shared>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 012b34f08..6d9918b76 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -87,8 +87,8 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph += NonlinearEquality(X(1), pose1); - graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.emplace_shared>(X(1), pose1); + graph.emplace_shared>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); std::shared_ptr fg = graph.linearize(config); @@ -115,7 +115,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.emplace_shared>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); std::shared_ptr fg = graph.linearize(config); diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index d7746e08d..344a1f56c 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -59,8 +59,8 @@ TEST(Marginals, planarSLAMmarginals) { SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry - graph += BetweenFactor(x1, x2, odometry, odometryNoise); - graph += BetweenFactor(x2, x3, odometry, odometryNoise); + graph.emplace_shared>(x1, x2, odometry, odometryNoise); + graph.emplace_shared>(x2, x3, odometry, odometryNoise); /* add measurements */ // general noisemodel for measurements @@ -75,9 +75,9 @@ TEST(Marginals, planarSLAMmarginals) { range32 = 2.0; // create bearing/range factors - graph += BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise); - graph += BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise); - graph += BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise); + graph.emplace_shared>(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared>(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared>(x3, l2, bearing32, range32, measurementNoise); // linearization point for marginals Values soln; @@ -195,9 +195,9 @@ TEST(Marginals, planarSLAMmarginals) { TEST(Marginals, order) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3)); - fg += BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); - fg += BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); - fg += BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg.emplace_shared>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg.emplace_shared>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg.emplace_shared>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); Values vals; vals.insert(0, Pose2()); @@ -208,31 +208,31 @@ TEST(Marginals, order) { vals.insert(100, Point2(0,1)); vals.insert(101, Point2(1,1)); - fg += BearingRangeFactor(0, 100, + fg.emplace_shared>(0, 100, vals.at(0).bearing(vals.at(100)), vals.at(0).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(0, 101, + fg.emplace_shared>(0, 101, vals.at(0).bearing(vals.at(101)), vals.at(0).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(1, 100, + fg.emplace_shared>(1, 100, vals.at(1).bearing(vals.at(100)), vals.at(1).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(1, 101, + fg.emplace_shared>(1, 101, vals.at(1).bearing(vals.at(101)), vals.at(1).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(2, 100, + fg.emplace_shared>(2, 100, vals.at(2).bearing(vals.at(100)), vals.at(2).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(2, 101, + fg.emplace_shared>(2, 101, vals.at(2).bearing(vals.at(101)), vals.at(2).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(3, 100, + fg.emplace_shared>(3, 100, vals.at(3).bearing(vals.at(100)), vals.at(3).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(3, 101, + fg.emplace_shared>(3, 101, vals.at(3).bearing(vals.at(101)), vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index efc0020a5..73eb2a2d9 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -193,11 +193,10 @@ TEST ( NonlinearEquality, allow_error_optimize ) { Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; - PoseNLE nle(key1, feasible1, error_gain); // add to a graph NonlinearFactorGraph graph; - graph += nle; + graph.emplace_shared(key1, feasible1, error_gain); // initialize away from the ideal Pose2 initPose(0.0, 2.0, 3.0); @@ -227,16 +226,13 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { Pose2 initPose(0.0, 2.0, 3.0); init.insert(key1, initPose); + // add nle to a graph double error_gain = 500.0; - PoseNLE nle(key1, feasible1, error_gain); - - // create a soft prior that conflicts - PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1)); - - // add to a graph NonlinearFactorGraph graph; - graph += nle; - graph += prior; + graph.emplace_shared(key1, feasible1, error_gain); + + // add a soft prior that conflicts + graph.emplace_shared(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1)); // optimize Ordering ordering; @@ -440,17 +436,17 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Symbol x1('x', 1), x2('x', 2); Symbol l1('l', 1), l2('l', 2); Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); - graph += eq2D::UnaryEqualityConstraint(pt_x1, x1); - graph += eq2D::UnaryEqualityConstraint(pt_x2, x2); + graph.emplace_shared(pt_x1, x1); + graph.emplace_shared(pt_x2, x2); Point2 z1(0.0, 5.0); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - graph += simulated2D::Measurement(z1, sigma, x1, l1); + graph.emplace_shared(z1, sigma, x1, l1); Point2 z2(-4.0, 0.0); - graph += simulated2D::Measurement(z2, sigma, x2, l2); + graph.emplace_shared(z2, sigma, x2, l2); - graph += eq2D::PointEqualityConstraint(l1, l2); + graph.emplace_shared(l1, l2); Values initialEstimate; initialEstimate.insert(x1, pt_x1); @@ -480,20 +476,20 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // constant constraint on x1 Point2 pose1(1.0, 1.0); - graph += eq2D::UnaryEqualityConstraint(pose1, x1); + graph.emplace_shared(pose1, x1); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2, 0.1); // measurement from x1 to l1 Point2 z1(0.0, 5.0); - graph += simulated2D::Measurement(z1, sigma, x1, l1); + graph.emplace_shared(z1, sigma, x1, l1); // measurement from x2 to l2 Point2 z2(-4.0, 0.0); - graph += simulated2D::Measurement(z2, sigma, x2, l2); + graph.emplace_shared(z2, sigma, x2, l2); // equality constraint between l1 and l2 - graph += eq2D::PointEqualityConstraint(l1, l2); + graph.emplace_shared(l1, l2); // create an initial estimate Values initialEstimate; @@ -542,18 +538,18 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { NonlinearFactorGraph graph; // create equality constraints for poses - graph += NonlinearEquality(key_x1, leftCamera.pose()); - graph += NonlinearEquality(key_x2, rightCamera.pose()); + graph.emplace_shared>(key_x1, leftCamera.pose()); + graph.emplace_shared>(key_x2, rightCamera.pose()); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(2); - graph += GenericProjectionFactor( + graph.emplace_shared>( leftCamera.project(landmark), vmodel, key_x1, key_l1, shK); - graph += GenericProjectionFactor( + graph.emplace_shared>( rightCamera.project(landmark), vmodel, key_x2, key_l2, shK); // add equality constraint saying there is only one point - graph += NonlinearEquality2(key_l1, key_l2); + graph.emplace_shared>(key_l1, key_l2); // create initial data Point3 landmark1(0.5, 5, 0); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 466a6e96e..305cd963a 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -162,8 +162,8 @@ TEST( NonlinearFactorGraph, rekey ) // updated measurements Point2 z3(0, -1), z4(-1.5, -1.); SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); - expRekey += simulated2D::Measurement(z3, sigma0_2, X(1), L(4)); - expRekey += simulated2D::Measurement(z4, sigma0_2, X(2), L(4)); + expRekey.emplace_shared(z3, sigma0_2, X(1), L(4)); + expRekey.emplace_shared(z4, sigma0_2, X(2), L(4)); EXPECT(assert_equal(expRekey, actRekey)); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 4249f5bc4..4ffdbdafe 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -36,7 +36,7 @@ TEST(testNonlinearISAM, markov_chain ) { // create initial graph Pose2 cur_pose; // start at origin NonlinearFactorGraph start_factors; - start_factors += NonlinearEquality(0, cur_pose); + start_factors.emplace_shared>(0, cur_pose); Values init; Values expected; @@ -50,7 +50,7 @@ TEST(testNonlinearISAM, markov_chain ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { NonlinearFactorGraph new_factors; - new_factors += BetweenFactor(i-1, i, z, model); + new_factors.emplace_shared>(i-1, i, z, model); Values new_init; cur_pose = cur_pose.compose(z); @@ -84,7 +84,7 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { // create initial graph Pose2 cur_pose; // start at origin NonlinearFactorGraph start_factors; - start_factors += NonlinearEquality(0, cur_pose); + start_factors.emplace_shared>(0, cur_pose); Values init; Values expected; @@ -106,7 +106,7 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { NonlinearFactorGraph new_factors; - new_factors += BetweenFactor(i-1, i, z, model3); + new_factors.emplace_shared>(i-1, i, z, model3); Values new_init; cur_pose = cur_pose.compose(z); @@ -161,7 +161,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { // create initial graph Pose2 cur_pose; // start at origin NonlinearFactorGraph start_factors; - start_factors += NonlinearEquality(0, cur_pose); + start_factors.emplace_shared>(0, cur_pose); Values init; Values expected; @@ -183,7 +183,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { NonlinearFactorGraph new_factors; - new_factors += BetweenFactor(i-1, i, z, model3); + new_factors.emplace_shared>(i-1, i, z, model3); Values new_init; cur_pose = cur_pose.compose(z); @@ -204,7 +204,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { // Reconnect with observation later if (i == 15) { - new_factors += BearingRangeFactor( + new_factors.emplace_shared>( i, lm1, cur_pose.bearing(landmark1), cur_pose.range(landmark1), model2); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index b73ac7376..cac25c246 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -190,7 +190,7 @@ TEST( NonlinearOptimizer, Factorization ) NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.emplace_shared>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; ordering.push_back(X(1)); @@ -249,9 +249,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); - fg += BetweenFactor(0, 1, Pose2(1, 0, M_PI / 2), + fg.emplace_shared>(0, 1, Pose2(1, 0, M_PI / 2), noiseModel::Isotropic::Sigma(3, 1)); - fg += BetweenFactor(1, 2, Pose2(1, 0, M_PI / 2), + fg.emplace_shared>(1, 2, Pose2(1, 0, M_PI / 2), noiseModel::Isotropic::Sigma(3, 1)); Values init; @@ -352,10 +352,10 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); - fg += BetweenFactor(0, 1, Pose2(1,1.1,M_PI/4), + fg.emplace_shared>(0, 1, Pose2(1,1.1,M_PI/4), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(1,0.9,M_PI/2), + fg.emplace_shared>(0, 1, Pose2(1,0.9,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0), noiseModel::Isotropic::Sigma(3,1))); @@ -383,13 +383,13 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { NonlinearFactorGraph fg; fg.addPrior(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); - fg += BetweenFactor(0, 1, Point2(1,1.8), + fg.emplace_shared>(0, 1, Point2(1,1.8), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), noiseModel::Isotropic::Sigma(2,1))); - fg += BetweenFactor(0, 1, Point2(1,0.9), + fg.emplace_shared>(0, 1, Point2(1,0.9), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), noiseModel::Isotropic::Sigma(2,1))); - fg += BetweenFactor(0, 1, Point2(1,90), + fg.emplace_shared>(0, 1, Point2(1,90), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), noiseModel::Isotropic::Sigma(2,1))); @@ -417,16 +417,16 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); - fg += BetweenFactor(0, 1, Pose2(0,9, M_PI/2), + fg.emplace_shared>(0, 1, Pose2(0,9, M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(0, 11, M_PI/2), + fg.emplace_shared>(0, 1, Pose2(0, 11, M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(0, 10, M_PI/2), + fg.emplace_shared>(0, 1, Pose2(0, 10, M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(0,9, 0), + fg.emplace_shared>(0, 1, Pose2(0,9, 0), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); @@ -495,7 +495,7 @@ TEST(NonlinearOptimizer, disconnected_graph) { NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); - graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.emplace_shared>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize())); @@ -541,7 +541,7 @@ TEST(NonlinearOptimizer, subclass_solver) { NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); - graph += BetweenFactor(X(1), X(2), Pose2(1.5, 0., 0.), + graph.emplace_shared>(X(1), X(2), Pose2(1.5, 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 9497c00d7..bc9f9e608 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -84,13 +84,13 @@ TEST( GaussianFactorGraphSystem, multiply_getb) // Create a Gaussian Factor Graph GaussianFactorGraph simpleGFG; SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); // Create a dummy-preconditioner and a GaussianFactorGraphSystem DummyPreconditioner dummyPreconditioner; diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 8e3c6dcc5..ecdf36322 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -39,7 +39,7 @@ TEST( PCGsolver, verySimpleLinearSystem) { // Create a Gaussian Factor Graph GaussianFactorGraph simpleGFG; - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); // Exact solution already known VectorValues exactSolution; @@ -81,13 +81,13 @@ TEST(PCGSolver, simpleLinearSystem) { GaussianFactorGraph simpleGFG; //SharedDiagonal unit2 = noiseModel::Unit::Create(2); SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); //simpleGFG.print("system"); // Expected solution diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 53c4e16de..5cb31ef0a 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -31,6 +31,8 @@ using namespace gtsam; typedef BetweenFactor Between; typedef NonlinearFactorGraph Graph; +using symbol_shorthand::R; + /* ************************************************************************* */ TEST(Rot3, optimize) { @@ -38,11 +40,11 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); + fg.addPrior(R(0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); for(int j=0; j<6; ++j) { - truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); - initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg += Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01)); + truth.insert(R(j), Rot3::Rz(M_PI/3.0 * double(j))); + initial.insert(R(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); + fg.emplace_shared(R(j), R((j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01)); } Values final = GaussNewtonOptimizer(fg, initial).optimize();