From a9971fd2fd1c1bbe7bc54a617a53e4c4baff781e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Feb 2023 01:12:51 -0800 Subject: [PATCH] More emplace_shared usage --- gtsam/hybrid/tests/testHybridEstimation.cpp | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 5 +-- gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 6 +-- .../symbolic/tests/testSymbolicBayesTree.cpp | 4 +- .../tests/testSymbolicEliminationTree.cpp | 21 +++++----- .../tests/testSymbolicFactorGraph.cpp | 8 ++-- gtsam/symbolic/tests/testSymbolicISAM.cpp | 4 +- .../dynamics/tests/testIMUSystem.cpp | 42 +++++++++---------- .../ConcurrentFilteringAndSmoothing.cpp | 2 +- .../slam/SmartStereoProjectionFactor.h | 5 ++- .../slam/tests/testInvDepthFactor3.cpp | 9 ++-- .../slam/tests/testMultiProjectionFactor.cpp | 6 +-- 12 files changed, 56 insertions(+), 58 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 1568e49d0..5f6462b68 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -44,7 +44,7 @@ using symbol_shorthand::X; Ordering getOrdering(HybridGaussianFactorGraph& factors, const HybridGaussianFactorGraph& newFactors) { - factors += newFactors; + factors.push_back(newFactors); // Get all the discrete keys from the factors KeySet allDiscrete = factors.discreteKeySet(); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 553791385..2342994c9 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -483,10 +483,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { actualJacobian.augmentedInformation(), 1e-9)); // Construct from GaussianFactorGraph - GaussianFactorGraph gfg1; - gfg1 += expected; - GaussianFactorGraph gfg2; - gfg2 += actual; + GaussianFactorGraph gfg1 {expected}, gfg2 {actual}; HessianFactor hessian1(gfg1), hessian2(gfg2); EXPECT(assert_equal(hessian1, hessian2, 1e-9)); } diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 64caf399d..bf2b6a214 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -83,9 +83,9 @@ TEST(SymbolicBayesNet, Dot) { using symbol_shorthand::A; using symbol_shorthand::X; SymbolicBayesNet bn; - bn += SymbolicConditional(X(3), X(2), A(2)); - bn += SymbolicConditional(X(2), X(1), A(1)); - bn += SymbolicConditional(X(1)); + bn.emplace_shared(X(3), X(2), A(2)); + bn.emplace_shared(X(2), X(1), A(1)); + bn.emplace_shared(X(1)); DotWriter writer; writer.positionHints.emplace('a', 2); diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 6051fa95f..e4a47bdfb 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -257,8 +257,8 @@ TEST(BayesTree, removeTop) { // Check expected outcome SymbolicBayesNet expected; - expected += SymbolicConditional::FromKeys(Keys(_E_)(_L_)(_B_), 3); - expected += SymbolicConditional::FromKeys(Keys(_S_)(_B_)(_L_), 1); + expected.add(SymbolicConditional::FromKeys(Keys(_E_)(_L_)(_B_), 3)); + expected.add(SymbolicConditional::FromKeys(Keys(_S_)(_B_)(_L_), 1)); CHECK(assert_equal(expected, bn)); SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]}; diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index c93eb8593..47ef754a6 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -110,19 +110,20 @@ TEST(EliminationTree, Create2) { // \ | // l3 SymbolicFactorGraph graph; + graph.emplace_shared(X(1), L(1)); + graph.emplace_shared(X(1), X(2)); + graph.emplace_shared(X(2), L(1)); + graph.emplace_shared(X(2), X(3)); + graph.emplace_shared(X(3), X(4)); + graph.emplace_shared(X(4), L(2)); + graph.emplace_shared(X(4), X(5)); + graph.emplace_shared(L(2), X(5)); + graph.emplace_shared(X(4), L(3)); + graph.emplace_shared(X(5), L(3)); + auto binary = [](Key j1, Key j2) -> SymbolicFactor { return SymbolicFactor(j1, j2); }; - graph += binary(X(1), L(1)); - graph += binary(X(1), X(2)); - graph += binary(X(2), L(1)); - graph += binary(X(2), X(3)); - graph += binary(X(3), X(4)); - graph += binary(X(4), L(2)); - graph += binary(X(4), X(5)); - graph += binary(L(2), X(5)); - graph += binary(X(4), L(3)); - graph += binary(X(5), L(3)); SymbolicEliminationTree expected = EliminationTreeTester::MakeTree( // ChildNodes( // diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 841d06b51..8afb506ab 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -292,9 +292,9 @@ TEST(SymbolicFactorGraph, constructFromBayesNet) { // create Bayes Net SymbolicBayesNet bayesNet; - bayesNet += SymbolicConditional(0, 1, 2); - bayesNet += SymbolicConditional(1, 2); - bayesNet += SymbolicConditional(1); + bayesNet.emplace_shared(0, 1, 2); + bayesNet.emplace_shared(1, 2); + bayesNet.emplace_shared(1); // create actual factor graph from a Bayes Net SymbolicFactorGraph actual(bayesNet); @@ -349,7 +349,7 @@ TEST(SymbolicFactorGraph, push_back) { TEST(SymbolicFactorGraph, add_factors) { SymbolicFactorGraph fg1; fg1.push_factor(10); - fg1 += SymbolicFactor::shared_ptr(); // empty slot! + fg1.push_back(SymbolicFactor::shared_ptr()); // empty slot! fg1.push_factor(11); SymbolicFactorGraph fg2; diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index e84af28a3..c09f6f67e 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -79,8 +79,8 @@ TEST( SymbolicISAM, iSAM ) // Now we modify the Bayes tree by inserting a new factor over B and S SymbolicFactorGraph fullGraph; - fullGraph += asiaGraph; - fullGraph += SymbolicFactor(_B_, _S_); + fullGraph.push_back(asiaGraph); + fullGraph.emplace_shared(_B_, _S_); // This ordering is chosen to match the one chosen by COLAMD during the ISAM update Ordering ordering {_X_, _B_, _S_, _E_, _L_, _T_}; diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index c7e46e400..b7206264a 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -33,20 +33,20 @@ TEST(testIMUSystem, instantiations) { // just checking for compilation PoseRTV x1_v; - gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1); - gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3); - gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6); - gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9); + SharedNoiseModel model1 = noiseModel::Unit::Create(1); + SharedNoiseModel model3 = noiseModel::Unit::Create(3); + SharedNoiseModel model6 = noiseModel::Unit::Create(6); + SharedNoiseModel model9 = noiseModel::Unit::Create(9); Vector accel = Vector::Ones(3), gyro = Vector::Ones(3); IMUFactor imu(accel, gyro, 0.01, x1, x2, model6); FullIMUFactor full_imu(accel, gyro, 0.01, x1, x2, model9); - NonlinearEquality poseHardPrior(x1, x1_v); - BetweenFactor odom(x1, x2, x1_v, model9); - RangeFactor range(x1, x2, 1.0, model1); + NonlinearEquality poseHardPrior(x1, x1_v); + BetweenFactor odom(x1, x2, x1_v, model9); + RangeFactor range(x1, x2, 1.0, model1); VelocityConstraint constraint(x1, x2, 0.1, 10000); - PriorFactor posePrior(x1, x1_v, model9); + PriorFactor posePrior(x1, x1_v, model9); DHeightPrior heightPrior(x1, 0.1, model1); VelocityPrior velPrior(x1, Vector::Ones(3), model3); } @@ -68,13 +68,13 @@ TEST( testIMUSystem, optimize_chain ) { // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; - graph += NonlinearEquality(x1, pose1); - graph += IMUFactor(imu12, dt, x1, x2, model); - graph += IMUFactor(imu23, dt, x2, x3, model); - graph += IMUFactor(imu34, dt, x3, x4, model); - graph += VelocityConstraint(x1, x2, dt); - graph += VelocityConstraint(x2, x3, dt); - graph += VelocityConstraint(x3, x4, dt); + graph.emplace_shared>(x1, pose1); + graph.emplace_shared>(imu12, dt, x1, x2, model); + graph.emplace_shared>(imu23, dt, x2, x3, model); + graph.emplace_shared>(imu34, dt, x3, x4, model); + graph.emplace_shared(x1, x2, dt); + graph.emplace_shared(x2, x3, dt); + graph.emplace_shared(x3, x4, dt); // ground truth values Values true_values; @@ -114,10 +114,10 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; - graph += NonlinearEquality(x1, pose1); - graph += FullIMUFactor(imu12, dt, x1, x2, model); - graph += FullIMUFactor(imu23, dt, x2, x3, model); - graph += FullIMUFactor(imu34, dt, x3, x4, model); + graph.emplace_shared>(x1, pose1); + graph.emplace_shared>(imu12, dt, x1, x2, model); + graph.emplace_shared>(imu23, dt, x2, x3, model); + graph.emplace_shared>(imu34, dt, x3, x4, model); // ground truth values Values true_values; @@ -156,7 +156,7 @@ TEST( testIMUSystem, linear_trajectory) { Values true_traj, init_traj; NonlinearFactorGraph graph; - graph += NonlinearEquality(x0, start); + graph.emplace_shared>(x0, start); true_traj.insert(x0, start); init_traj.insert(x0, start); @@ -165,7 +165,7 @@ TEST( testIMUSystem, linear_trajectory) { for (size_t i=1; i(accel - g, gyro, dt, xA, xB, model); + graph.emplace_shared>(accel - g, gyro, dt, xA, xB, model); true_traj.insert(xB, cur_pose); init_traj.insert(xB, PoseRTV()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 9dc5b3fad..7bbcdd897 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -78,7 +78,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) { - marginalFactors += std::make_shared(gaussianFactor, theta); + marginalFactors.emplace_shared(gaussianFactor, theta); } return marginalFactors; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 0893af75e..52d6eda05 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -30,7 +30,10 @@ #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include +#endif + #include #include @@ -501,7 +504,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index afd51e1f6..64af687d4 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -62,10 +62,9 @@ TEST( InvDepthFactor, optimize) { gtsam::NonlinearFactorGraph graph; Values initial; - InverseDepthFactor::shared_ptr factor(new InverseDepthFactor(expected_uv, sigma, - Symbol('x',1), Symbol('l',1), Symbol('d',1), K)); - graph.push_back(factor); - graph += PoseConstraint(Symbol('x',1),level_pose); + graph.emplace_shared(expected_uv, sigma, + Symbol('x',1), Symbol('l',1), Symbol('d',1), K); + graph.emplace_shared(Symbol('x', 1), level_pose); initial.insert(Symbol('x',1), level_pose); initial.insert(Symbol('l',1), inv_landmark); initial.insert(Symbol('d',1), inv_depth); @@ -91,7 +90,7 @@ TEST( InvDepthFactor, optimize) { Symbol('x',2), Symbol('l',1),Symbol('d',1),K)); graph.push_back(factor1); - graph += PoseConstraint(Symbol('x',2),right_pose); + graph.emplace_shared(Symbol('x',2),right_pose); initial.insert(Symbol('x',2), right_pose); diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 141a50465..1d438b457 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -73,10 +73,8 @@ TEST( MultiProjectionFactor, create ){ views.insert(x2); views.insert(x3); - MultiProjectionFactor mpFactor(n_measPixel, noiseProjection, views, l1, K); - graph += mpFactor; - - + graph.emplace_shared>( + n_measPixel, noiseProjection, views, l1, K); }