diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.h b/gtsam/hybrid/GaussianHybridFactorGraph.h index 5ca8c38a20..6bf4141da1 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.h +++ b/gtsam/hybrid/GaussianHybridFactorGraph.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -47,8 +48,9 @@ struct EliminationTraits { typedef HybridEliminationTree JunctionTreeType; /// The function type that does a single elimination step on a variable. - static std::pair DefaultEliminate( - const GaussianHybridFactorGraph& factors, const Ordering& ordering) { + static std::pair + DefaultEliminate(const GaussianHybridFactorGraph& factors, + const Ordering& ordering) { return EliminateHybrid(factors, ordering); } }; diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index b9fcb8c814..f6a61a0779 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -20,11 +20,11 @@ #include #include -#include #include -#include -#include +#include #include +#include +#include namespace gtsam { using MotionModel = BetweenFactor; @@ -52,7 +52,7 @@ struct Switching { // Create hybrid factor graph. // Add a prior on X(1). - auto prior = boost::make_shared < PriorFactor < double >> ( + auto prior = boost::make_shared>( X(1), 0, noiseModel::Isotropic::Sigma(1, prior_sigma)); nonlinearFactorGraph.push_nonlinear(prior); @@ -67,7 +67,7 @@ struct Switching { // Add measurement factors auto measurement_noise = noiseModel::Isotropic::Sigma(1, 0.1); for (size_t k = 1; k <= K; k++) { - nonlinearFactorGraph.emplace_nonlinear < PriorFactor < double >> ( + nonlinearFactorGraph.emplace_nonlinear>( X(k), 1.0 * (k - 1), measurement_noise); } @@ -83,16 +83,16 @@ struct Switching { } // Create motion models for a given time step - static std::vector motionModels(size_t k, - double sigma = 1.0) { + static std::vector motionModels(size_t k, + double sigma = 1.0) { using symbol_shorthand::M; using symbol_shorthand::X; auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); auto still = - boost::make_shared(X(k), X(k + 1), 0.0, noise_model), - moving = - boost::make_shared(X(k), X(k + 1), 1.0, noise_model); + boost::make_shared(X(k), X(k + 1), 0.0, noise_model), + moving = + boost::make_shared(X(k), X(k + 1), 1.0, noise_model); return {still, moving}; } @@ -121,4 +121,4 @@ struct Switching { } }; -} +} // namespace gtsam