From a77b5bc1d716f355aa7882b63feda61e890370e2 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Fri, 20 Jan 2023 16:57:33 -0800 Subject: [PATCH] boost::variant -> std::variant --- gtsam/inference/BayesTree-inst.h | 7 ++- gtsam/inference/BayesTreeCliqueBase-inst.h | 5 ++- .../inference/EliminateableFactorGraph-inst.h | 43 ++++++++++++------- gtsam/inference/EliminateableFactorGraph.h | 17 +++++--- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 2 +- gtsam/nonlinear/ISAM2-impl.h | 14 +++--- gtsam/nonlinear/ISAM2.cpp | 20 +++++---- gtsam/nonlinear/ISAM2Params.h | 22 +++++----- .../tests/testSymbolicFactorGraph.cpp | 3 +- .../tests/testConcurrentIncrementalFilter.cpp | 20 ++++----- 10 files changed, 92 insertions(+), 61 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 64cca5546..e6d0b778f 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -26,6 +26,7 @@ #include #include +#include namespace gtsam { @@ -277,8 +278,9 @@ namespace gtsam { FactorGraphType cliqueMarginal = clique->marginal2(function); // Now, marginalize out everything that is not variable j + auto ordering = Ordering{j}; BayesNetType marginalBN = - *cliqueMarginal.marginalMultifrontalBayesNet(Ordering{j}, function); + *cliqueMarginal.marginalMultifrontalBayesNet(std::cref(ordering), function); // The Bayes net should contain only one conditional for variable j, so return it return marginalBN.front(); @@ -400,8 +402,9 @@ namespace gtsam { gttoc(Disjoint_marginals); } + auto ordering = Ordering{j1, j2}; // now, marginalize out everything that is not variable j1 or j2 - return p_BC1C2.marginalMultifrontalBayesNet(Ordering{j1, j2}, function); + return p_BC1C2.marginalMultifrontalBayesNet(std::cref(ordering), function); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 62ed0b90f..25633ad30 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -20,6 +20,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ @@ -176,8 +178,9 @@ namespace gtsam { // The variables we want to keepSet are exactly the ones in S KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); + auto ordering = Ordering(indicesS); auto separatorMarginal = - p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); + p_Cp.marginalMultifrontalBayesNet(std::cref(ordering), function); cachedSeparatorMarginal_ = *separatorMarginal; } } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index eadb9715e..5673379cd 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -21,6 +21,15 @@ #include #include +// some helper functions +namespace { + // A function to take a reference_wrapper object and return the underlying pointer + template + T* get_pointer(std::reference_wrapper ref) { + return &ref.get(); + } +} + namespace gtsam { /* ************************************************************************* */ @@ -226,7 +235,7 @@ namespace gtsam { template std::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( - boost::variant variables, + OrderingKeyVectorVariant variables, const Eliminate& function, OptionalVariableIndex variableIndex) const { if(!variableIndex) { @@ -236,10 +245,10 @@ namespace gtsam { } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); + bool unmarginalizedAreOrdered = (std::get_if(&variables) != nullptr); + const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered + ? get_pointer(std::get(variables)) + : get_pointer(std::get(variables)); Ordering totalOrdering = Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); @@ -250,7 +259,7 @@ namespace gtsam { Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); // Call this function again with the computed orderings - return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, variableIndex); + return marginalMultifrontalBayesNet(std::cref(marginalVarsOrdering), marginalizationOrdering, function, variableIndex); } } @@ -258,7 +267,7 @@ namespace gtsam { template std::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( - boost::variant variables, + OrderingKeyVectorVariant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -273,8 +282,9 @@ namespace gtsam { const auto [bayesTree, factorGraph] = eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); - if(const Ordering* varsAsOrdering = boost::get(&variables)) + if(std::get_if(&variables)) { + const Ordering* varsAsOrdering = get_pointer(std::get(variables)); // An ordering was also provided for the unmarginalized variables, so we can also // eliminate them in the order requested. return factorGraph->eliminateSequential(*varsAsOrdering, function); @@ -291,7 +301,7 @@ namespace gtsam { template std::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( - boost::variant variables, + OrderingKeyVectorVariant variables, const Eliminate& function, OptionalVariableIndex variableIndex) const { if(!variableIndex) { @@ -301,10 +311,10 @@ namespace gtsam { } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); + bool unmarginalizedAreOrdered = (std::get_if(&variables) != 0); + const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered + ? get_pointer(std::get(variables)) + : get_pointer(std::get(variables)); Ordering totalOrdering = Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); @@ -315,7 +325,7 @@ namespace gtsam { Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); // Call this function again with the computed orderings - return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, variableIndex); + return marginalMultifrontalBayesTree(std::cref(marginalVarsOrdering), marginalizationOrdering, function, variableIndex); } } @@ -323,7 +333,7 @@ namespace gtsam { template std::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( - boost::variant variables, + OrderingKeyVectorVariant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -338,8 +348,9 @@ namespace gtsam { const auto [bayesTree, factorGraph] = eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); - if(const Ordering* varsAsOrdering = boost::get(&variables)) + if(std::get_if(&variables)) { + const Ordering* varsAsOrdering = get_pointer(std::get(variables)); // An ordering was also provided for the unmarginalized variables, so we can also // eliminate them in the order requested. return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 561c478ff..777c0f505 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -22,12 +22,19 @@ #include #include #include -#include +#include #include #include namespace gtsam { + // Creating an alias for the variant type since it is verbose + template + using ref_wrap = std::reference_wrapper; + using OrderingConstRef = std::reference_wrapper; + using KeyVectorConstRef = std::reference_wrapper; + using OrderingKeyVectorVariant = + std::variant; /// Traits class for eliminateable factor graphs, specifies the types that result from /// elimination, etc. This must be defined for each factor graph that inherits from @@ -225,7 +232,7 @@ namespace gtsam { * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ std::shared_ptr marginalMultifrontalBayesNet( - boost::variant variables, + OrderingKeyVectorVariant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -240,7 +247,7 @@ namespace gtsam { * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ std::shared_ptr marginalMultifrontalBayesNet( - boost::variant variables, + OrderingKeyVectorVariant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -255,7 +262,7 @@ namespace gtsam { * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ std::shared_ptr marginalMultifrontalBayesTree( - boost::variant variables, + OrderingKeyVectorVariant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -270,7 +277,7 @@ namespace gtsam { * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ std::shared_ptr marginalMultifrontalBayesTree( - boost::variant variables, + OrderingKeyVectorVariant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index f252454ef..917a1053b 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -38,7 +38,7 @@ namespace gtsam { Ordering lastKeyAsOrdering; lastKeyAsOrdering += lastKey; const GaussianConditional::shared_ptr marginal = - linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front(); + linearFactorGraph.marginalMultifrontalBayesNet(std::cref(lastKeyAsOrdering))->front(); // Extract the current estimate of x1,P1 VectorValues result = marginal->solve(VectorValues()); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index ad53b7972..e9a9696eb 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -32,6 +32,7 @@ #include #include #include +#include namespace gtsam { @@ -313,13 +314,14 @@ struct GTSAM_EXPORT UpdateImpl { const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; for (const ISAM2::sharedClique& root : roots) { - if (relinearizeThreshold.type() == typeid(double)) + if (std::holds_alternative(relinearizeThreshold)) { CheckRelinearizationRecursiveDouble( - boost::get(relinearizeThreshold), delta, root, &relinKeys); - else if (relinearizeThreshold.type() == typeid(FastMap)) + std::get(relinearizeThreshold), delta, root, &relinKeys); + } else if (std::holds_alternative>(relinearizeThreshold)) { CheckRelinearizationRecursiveMap( - boost::get >(relinearizeThreshold), delta, + std::get >(relinearizeThreshold), delta, root, &relinKeys); + } } return relinKeys; } @@ -340,13 +342,13 @@ struct GTSAM_EXPORT UpdateImpl { const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; - if (const double* threshold = boost::get(&relinearizeThreshold)) { + if (const double* threshold = std::get_if(&relinearizeThreshold)) { for (const VectorValues::KeyValuePair& key_delta : delta) { double maxDelta = key_delta.second.lpNorm(); if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); } } else if (const FastMap* thresholds = - boost::get >(&relinearizeThreshold)) { + std::get_if >(&relinearizeThreshold)) { for (const VectorValues::KeyValuePair& key_delta : delta) { const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 579231151..727a8befd 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; @@ -38,16 +39,18 @@ template class BayesTree; /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { - if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + if (std::holds_alternative(params_.optimizationParams)) { doglegDelta_ = - boost::get(params_.optimizationParams).initialDelta; + std::get(params_.optimizationParams).initialDelta; + } } /* ************************************************************************* */ ISAM2::ISAM2() : update_count_(0) { - if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + if (std::holds_alternative(params_.optimizationParams)) { doglegDelta_ = - boost::get(params_.optimizationParams).initialDelta; + std::get(params_.optimizationParams).initialDelta; + } } /* ************************************************************************* */ @@ -702,10 +705,10 @@ void ISAM2::marginalizeLeaves( // Marked const but actually changes mutable delta void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); - if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + if (std::holds_alternative(params_.optimizationParams)) { // If using Gauss-Newton, update with wildfireThreshold const ISAM2GaussNewtonParams& gaussNewtonParams = - boost::get(params_.optimizationParams); + std::get(params_.optimizationParams); const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); @@ -713,11 +716,10 @@ void ISAM2::updateDelta(bool forceFullSolve) const { effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); - - } else if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + } else if (std::holds_alternative(params_.optimizationParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = - boost::get(params_.optimizationParams); + std::get(params_.optimizationParams); const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index 029f66e52..bc79cd456 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { @@ -133,10 +134,10 @@ struct GTSAM_EXPORT ISAM2DoglegParams { typedef FastMap ISAM2ThresholdMap; typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; struct GTSAM_EXPORT ISAM2Params { - typedef boost::variant + typedef std::variant OptimizationParams; ///< Either ISAM2GaussNewtonParams or ///< ISAM2DoglegParams - typedef boost::variant > + typedef std::variant > RelinearizationThreshold; ///< Either a constant relinearization ///< threshold or a per-variable-type set of ///< thresholds @@ -254,20 +255,21 @@ struct GTSAM_EXPORT ISAM2Params { cout << str << "\n"; static const std::string kStr("optimizationParams: "); - if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - boost::get(optimizationParams).print(); - else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) - boost::get(optimizationParams).print(kStr); - else + if (std::holds_alternative(optimizationParams)) { + std::get(optimizationParams).print(); + } else if (std::holds_alternative(optimizationParams)) { + std::get(optimizationParams).print(kStr); + } else { cout << kStr << "{unknown type}\n"; + } cout << "relinearizeThreshold: "; - if (relinearizeThreshold.type() == typeid(double)) { - cout << boost::get(relinearizeThreshold) << "\n"; + if (std::holds_alternative(relinearizeThreshold)) { + cout << std::get(relinearizeThreshold) << "\n"; } else { cout << "{mapped}\n"; for (const ISAM2ThresholdMapValue& value : - boost::get(relinearizeThreshold)) { + std::get(relinearizeThreshold)) { cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; } diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 260cdcbcb..30a7bb943 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -129,8 +129,9 @@ TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional( 1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3)); + auto ordering = Ordering{0,1,2,3}; SymbolicBayesNet actual1 = - *simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3}); + *simpleTestGraph2.marginalMultifrontalBayesNet(std::cref(ordering)); EXPECT(assert_equal(expectedBayesNet, actual1)); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 7c6a08278..401dee762 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -468,7 +468,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) { // Create a set of optimizer parameters ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -594,7 +594,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 ) { // Create a set of optimizer parameters ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -641,7 +641,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_2 ) { // Create a set of optimizer parameters ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -711,7 +711,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 ) { // Create a set of optimizer parameters ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -798,7 +798,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 ) { // Create a set of optimizer parameters ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -893,7 +893,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) { // Create a set of optimizer parameters ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -1182,7 +1182,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) { // Create a set of optimizer parameters ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -1241,7 +1241,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) // we try removing the last factor ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -1300,7 +1300,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) // we try removing the first factor ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1; @@ -1357,7 +1357,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) // we try removing the last factor ISAM2Params parameters; - parameters.relinearizeThreshold = 0; + parameters.relinearizeThreshold = 0.; // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // default value for that is 10 (if you set that to zero the code will crash) parameters.relinearizeSkip = 1;