From 88129d9f456c1d36278ae9b90510a95c038714ed Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 6 Jan 2023 19:01:52 -0800 Subject: [PATCH] Remove HybridNonlinearFactor wrapper class as no longer needed --- gtsam/hybrid/HybridFactorGraph.h | 20 ----- gtsam/hybrid/HybridNonlinearFactor.cpp | 41 ---------- gtsam/hybrid/HybridNonlinearFactor.h | 73 ------------------ gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 21 +----- gtsam/hybrid/HybridNonlinearFactorGraph.h | 75 ------------------- gtsam/hybrid/MixtureFactor.h | 1 - gtsam/hybrid/tests/Switching.h | 11 +-- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- gtsam/hybrid/tests/testHybridEstimation.cpp | 4 +- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 32 ++++---- .../tests/testHybridNonlinearFactorGraph.cpp | 8 +- .../hybrid/tests/testHybridNonlinearISAM.cpp | 32 ++++---- 12 files changed, 49 insertions(+), 271 deletions(-) delete mode 100644 gtsam/hybrid/HybridNonlinearFactor.cpp delete mode 100644 gtsam/hybrid/HybridNonlinearFactor.h diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index e2322ee0b..8e1fa0123 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -99,10 +99,6 @@ class HybridFactorGraph : public FactorGraph { Base::push_back(hybridFactor); } - /// delete emplace_shared. - template - void emplace_shared(Args&&... args) = delete; - /// Construct a factor and add (shared pointer to it) to factor graph. template IsDiscrete emplace_discrete(Args&&... args) { @@ -119,22 +115,6 @@ class HybridFactorGraph : public FactorGraph { push_hybrid(factor); } - /** - * @brief Add a single factor shared pointer to the hybrid factor graph. - * Dynamically handles the factor type and assigns it to the correct - * underlying container. - * - * @param sharedFactor The factor to add to this factor graph. - */ - void push_back(const SharedFactor& sharedFactor) { - if (auto p = boost::dynamic_pointer_cast(sharedFactor)) { - push_discrete(p); - } - if (auto p = boost::dynamic_pointer_cast(sharedFactor)) { - push_hybrid(p); - } - } - /// Get all the discrete keys in the factor graph. const KeySet discreteKeys() const { KeySet discrete_keys; diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp deleted file mode 100644 index 5a1833d39..000000000 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file HybridNonlinearFactor.cpp - * @date May 28, 2022 - * @author Varun Agrawal - */ - -#include - -#include - -namespace gtsam { - -/* ************************************************************************* */ -HybridNonlinearFactor::HybridNonlinearFactor( - const NonlinearFactor::shared_ptr &other) - : Base(other->keys()), inner_(other) {} - -/* ************************************************************************* */ -bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); -} - -/* ************************************************************************* */ -void HybridNonlinearFactor::print(const std::string &s, - const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); - inner_->print("\n", formatter); -}; - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h deleted file mode 100644 index 56e30ba74..000000000 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file HybridNonlinearFactor.h - * @date May 28, 2022 - * @author Varun Agrawal - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -/** - * A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not - * have a diamond inheritance. - */ -class HybridNonlinearFactor : public HybridFactor { - private: - NonlinearFactor::shared_ptr inner_; - - public: - using Base = HybridFactor; - using This = HybridNonlinearFactor; - using shared_ptr = boost::shared_ptr; - - // Explicit conversion from a shared ptr of NonlinearFactor - explicit HybridNonlinearFactor(const NonlinearFactor::shared_ptr &other); - - /// @name Testable - /// @{ - - /// Check equality. - virtual bool equals(const HybridFactor &lf, double tol) const override; - - /// GTSAM print utility. - void print( - const std::string &s = "HybridNonlinearFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; - - /// @} - /// @name Standard Interface - /// @{ - - /// Return pointer to the internal nonlinear factor. - NonlinearFactor::shared_ptr inner() const { return inner_; } - - /// Error for HybridValues is not provided for nonlinear factor. - double error(const HybridValues &values) const override { - throw std::runtime_error( - "HybridNonlinearFactor::error(HybridValues) not implemented."); - } - - /// Linearize to a HybridGaussianFactor at the linearization point `c`. - boost::shared_ptr linearize(const Values &c) const { - return boost::make_shared(inner_->linearize(c)); - } - - /// @} -}; -} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 6ab1962d4..380469b45 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -20,17 +20,6 @@ namespace gtsam { -/* ************************************************************************* */ -void HybridNonlinearFactorGraph::add( - boost::shared_ptr factor) { - FactorGraph::add(boost::make_shared(factor)); -} - -/* ************************************************************************* */ -void HybridNonlinearFactorGraph::add(boost::shared_ptr factor) { - FactorGraph::add(boost::make_shared(factor)); -} - /* ************************************************************************* */ void HybridNonlinearFactorGraph::print(const std::string& s, const KeyFormatter& keyFormatter) const { @@ -66,14 +55,12 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( continue; } // Check if it is a nonlinear mixture factor - if (auto nlmf = dynamic_pointer_cast(f)) { + if (auto mf = dynamic_pointer_cast(f)) { const GaussianMixtureFactor::shared_ptr& gmf = - nlmf->linearize(continuousValues); + mf->linearize(continuousValues); linearFG->push_back(gmf); - } else if (auto nlhf = dynamic_pointer_cast(f)) { - // Nonlinear wrapper case: - const GaussianFactor::shared_ptr& gf = - nlhf->inner()->linearize(continuousValues); + } else if (auto nlf = dynamic_pointer_cast(f)) { + const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues); const auto hgf = boost::make_shared(gf); linearFG->push_back(hgf); } else if (dynamic_pointer_cast(f) || diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 25314cfd3..59921822e 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -37,16 +36,6 @@ namespace gtsam { */ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { protected: - /// Check if FACTOR type is derived from NonlinearFactor. - template - using IsNonlinear = typename std::enable_if< - std::is_base_of::value>::type; - - /// Check if T has a value_type derived from FactorType. - template - using HasDerivedValueType = typename std::enable_if< - std::is_base_of::value>::type; - public: using Base = HybridFactorGraph; using This = HybridNonlinearFactorGraph; ///< this class @@ -71,70 +60,6 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { /// @} - // Allow use of selected FactorGraph methods: - using Base::empty; - using Base::reserve; - using Base::size; - using Base::operator[]; - using Base::add; - using Base::resize; - - /** - * Add a nonlinear factor *pointer* to the internal nonlinear factor graph - * @param nonlinearFactor - boost::shared_ptr to the factor to add - */ - template - IsNonlinear push_nonlinear( - const boost::shared_ptr& nonlinearFactor) { - Base::push_back(boost::make_shared(nonlinearFactor)); - } - - /// Construct a factor and add (shared pointer to it) to factor graph. - template - IsNonlinear emplace_nonlinear(Args&&... args) { - auto factor = boost::allocate_shared( - Eigen::aligned_allocator(), std::forward(args)...); - push_nonlinear(factor); - } - - /** - * @brief Add a single factor shared pointer to the hybrid factor graph. - * Dynamically handles the factor type and assigns it to the correct - * underlying container. - * - * @tparam FACTOR The factor type template - * @param sharedFactor The factor to add to this factor graph. - */ - template - void push_back(const boost::shared_ptr& sharedFactor) { - if (auto p = boost::dynamic_pointer_cast(sharedFactor)) { - push_nonlinear(p); - } else { - Base::push_back(sharedFactor); - } - } - - /** - * Push back many factors as shared_ptr's in a container (factors are not - * copied) - */ - template - void push_back(const CONTAINER& container) { - Base::push_back(container.begin(), container.end()); - } - - /// Push back non-pointer objects in a container (factors are copied). - template - HasDerivedValueType push_back(const CONTAINER& container) { - Base::push_back(container.begin(), container.end()); - } - - /// Add a nonlinear factor as a shared ptr. - void add(boost::shared_ptr factor); - - /// Add a discrete factor as a shared ptr. - void add(boost::shared_ptr factor); - /// Print the factor graph. void print( const std::string& s = "HybridNonlinearFactorGraph", diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 5285dd191..38905b8a2 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -21,7 +21,6 @@ #include #include -#include #include #include #include diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index b232efbf2..385a7c3d5 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -136,6 +136,8 @@ struct Switching { std::vector measurements = {}, std::string discrete_transition_prob = "1/2 3/2") : K(K) { + using noiseModel::Isotropic; + // Create DiscreteKeys for binary K modes. for (size_t k = 0; k < K; k++) { modes.emplace_back(M(k), 2); @@ -150,9 +152,8 @@ struct Switching { // Create hybrid factor graph. // Add a prior on X(0). - auto prior = boost::make_shared>( - X(0), measurements.at(0), noiseModel::Isotropic::Sigma(1, prior_sigma)); - nonlinearFactorGraph.push_nonlinear(prior); + nonlinearFactorGraph.emplace_shared>( + X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma)); // Add "motion models". for (size_t k = 0; k < K - 1; k++) { @@ -167,9 +168,9 @@ struct Switching { } // Add measurement factors - auto measurement_noise = noiseModel::Isotropic::Sigma(1, prior_sigma); + auto measurement_noise = Isotropic::Sigma(1, prior_sigma); for (size_t k = 1; k < K; k++) { - nonlinearFactorGraph.emplace_nonlinear>( + nonlinearFactorGraph.emplace_shared>( X(k), measurements.at(k), measurement_noise); } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 9dc79369d..0149ff7ab 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -336,7 +336,7 @@ TEST(HybridBayesNet, Sampling) { auto one_motion = boost::make_shared>(X(0), X(1), 1, noise_model); std::vector factors = {zero_motion, one_motion}; - nfg.emplace_nonlinear>(X(0), 0.0, noise_model); + nfg.emplace_shared>(X(0), 0.0, noise_model); nfg.emplace_hybrid( KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 84f686c59..1f3d1079c 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -407,8 +407,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); // Add "measurement" factors: - nfg.emplace_nonlinear>(X(0), 0.0, noise_model); - nfg.emplace_nonlinear>(X(1), 1.0, noise_model); + nfg.emplace_shared>(X(0), 0.0, noise_model); + nfg.emplace_shared>(X(1), 1.0, noise_model); // Add mixture factor: DiscreteKey m(M(0), 2); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 1ce10b452..4f135f84f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -380,7 +380,7 @@ TEST(HybridGaussianISAM, NonTrivial) { Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin auto priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - fg.emplace_nonlinear>(X(0), prior, priorNoise); + fg.emplace_shared>(X(0), prior, priorNoise); // create a noise model for the landmark measurements auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); @@ -389,11 +389,11 @@ TEST(HybridGaussianISAM, NonTrivial) { // where X is the base link and W is the foot link. // Add connecting poses similar to PoseFactors in GTD - fg.emplace_nonlinear>(X(0), Y(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(X(0), Y(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Y(0), Z(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Z(0), W(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Z(0), W(0), Pose2(0, 1.0, 0), poseNoise); // Create initial estimate @@ -432,14 +432,14 @@ TEST(HybridGaussianISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), + fg.emplace_shared>(X(1), Y(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), + fg.emplace_shared>(Y(1), Z(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(1), W(1), Pose2(-1, 1, 0), + fg.emplace_shared>(Z(1), W(1), Pose2(-1, 1, 0), poseNoise); initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); @@ -472,14 +472,14 @@ TEST(HybridGaussianISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), + fg.emplace_shared>(X(2), Y(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), + fg.emplace_shared>(Y(2), Z(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), + fg.emplace_shared>(Z(2), W(2), Pose2(-2, 1, 0), poseNoise); initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); @@ -515,14 +515,14 @@ TEST(HybridGaussianISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=3 - fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), + fg.emplace_shared>(X(3), Y(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), + fg.emplace_shared>(Y(3), Z(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), + fg.emplace_shared>(Z(3), W(3), Pose2(-3, 1, 0), poseNoise); initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 83a71a7d7..4b2b76c59 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -54,7 +54,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { HybridNonlinearFactorGraph fg; // Add a simple prior factor to the nonlinear factor graph - fg.emplace_nonlinear>(X(0), 0, Isotropic::Sigma(1, 0.1)); + fg.emplace_shared>(X(0), 0, Isotropic::Sigma(1, 0.1)); // Linearization point Values linearizationPoint; @@ -683,7 +683,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin auto priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - fg.emplace_nonlinear>(X(0), prior, priorNoise); + fg.emplace_shared>(X(0), prior, priorNoise); using PlanarMotionModel = BetweenFactor; @@ -708,9 +708,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0; // Add Bearing-Range factors - fg.emplace_nonlinear>( + fg.emplace_shared>( X(0), L(0), bearing11, range11, measurementNoise); - fg.emplace_nonlinear>( + fg.emplace_shared>( X(1), L(1), bearing22, range22, measurementNoise); // Create (deliberately inaccurate) initial estimate diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 68a55abdd..d89909122 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -409,7 +409,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin auto priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - fg.emplace_nonlinear>(X(0), prior, priorNoise); + fg.emplace_shared>(X(0), prior, priorNoise); // create a noise model for the landmark measurements auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); @@ -418,11 +418,11 @@ TEST(HybridNonlinearISAM, NonTrivial) { // where X is the base link and W is the foot link. // Add connecting poses similar to PoseFactors in GTD - fg.emplace_nonlinear>(X(0), Y(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(X(0), Y(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Y(0), Z(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Z(0), W(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Z(0), W(0), Pose2(0, 1.0, 0), poseNoise); // Create initial estimate @@ -451,14 +451,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), + fg.emplace_shared>(X(1), Y(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), + fg.emplace_shared>(Y(1), Z(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(1), W(1), Pose2(-1, 1, 0), + fg.emplace_shared>(Z(1), W(1), Pose2(-1, 1, 0), poseNoise); initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); @@ -491,14 +491,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), + fg.emplace_shared>(X(2), Y(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), + fg.emplace_shared>(Y(2), Z(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), + fg.emplace_shared>(Z(2), W(2), Pose2(-2, 1, 0), poseNoise); initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); @@ -534,14 +534,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=3 - fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), + fg.emplace_shared>(X(3), Y(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), + fg.emplace_shared>(Y(3), Z(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), + fg.emplace_shared>(Z(3), W(3), Pose2(-3, 1, 0), poseNoise); initial.insert(X(3), Pose2(3.0, 0.0, 0.0));