boost::variant -> std::variant

release/4.3a0
kartik arcot 2023-01-20 16:57:33 -08:00 committed by Frank Dellaert
parent 6160759f13
commit a77b5bc1d7
10 changed files with 92 additions and 61 deletions

View File

@ -26,6 +26,7 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <fstream> #include <fstream>
#include <functional>
namespace gtsam { namespace gtsam {
@ -277,8 +278,9 @@ namespace gtsam {
FactorGraphType cliqueMarginal = clique->marginal2(function); FactorGraphType cliqueMarginal = clique->marginal2(function);
// Now, marginalize out everything that is not variable j // Now, marginalize out everything that is not variable j
auto ordering = Ordering{j};
BayesNetType marginalBN = 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 // The Bayes net should contain only one conditional for variable j, so return it
return marginalBN.front(); return marginalBN.front();
@ -400,8 +402,9 @@ namespace gtsam {
gttoc(Disjoint_marginals); gttoc(Disjoint_marginals);
} }
auto ordering = Ordering{j1, j2};
// now, marginalize out everything that is not variable j1 or 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);
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -20,6 +20,8 @@
#include <gtsam/inference/FactorGraph-inst.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <functional>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
@ -176,8 +178,9 @@ namespace gtsam {
// The variables we want to keepSet are exactly the ones in S // The variables we want to keepSet are exactly the ones in S
KeyVector indicesS(this->conditional()->beginParents(), KeyVector indicesS(this->conditional()->beginParents(),
this->conditional()->endParents()); this->conditional()->endParents());
auto ordering = Ordering(indicesS);
auto separatorMarginal = auto separatorMarginal =
p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); p_Cp.marginalMultifrontalBayesNet(std::cref(ordering), function);
cachedSeparatorMarginal_ = *separatorMarginal; cachedSeparatorMarginal_ = *separatorMarginal;
} }
} }

View File

@ -21,6 +21,15 @@
#include <gtsam/inference/EliminateableFactorGraph.h> #include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/inferenceExceptions.h> #include <gtsam/inference/inferenceExceptions.h>
// some helper functions
namespace {
// A function to take a reference_wrapper object and return the underlying pointer
template<typename T>
T* get_pointer(std::reference_wrapper<T> ref) {
return &ref.get();
}
}
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
@ -226,7 +235,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet( EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Eliminate& function, OptionalVariableIndex variableIndex) const const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
if(!variableIndex) { if(!variableIndex) {
@ -236,10 +245,10 @@ namespace gtsam {
} else { } else {
// No ordering was provided for the marginalized variables, so order them using constrained // No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD. // COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0); bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != nullptr);
const KeyVector* variablesOrOrdering = const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
unmarginalizedAreOrdered ? ? get_pointer(std::get<const OrderingConstRef>(variables))
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables); : get_pointer(std::get<const KeyVectorConstRef>(variables));
Ordering totalOrdering = Ordering totalOrdering =
Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
@ -250,7 +259,7 @@ namespace gtsam {
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings // 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<class FACTORGRAPH> template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet( EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Ordering& marginalizedVariableOrdering, const Ordering& marginalizedVariableOrdering,
const Eliminate& function, OptionalVariableIndex variableIndex) const const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
@ -273,8 +282,9 @@ namespace gtsam {
const auto [bayesTree, factorGraph] = const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables)) if(std::get_if<const OrderingConstRef>(&variables))
{ {
const Ordering* varsAsOrdering = get_pointer(std::get<const OrderingConstRef>(variables));
// An ordering was also provided for the unmarginalized variables, so we can also // An ordering was also provided for the unmarginalized variables, so we can also
// eliminate them in the order requested. // eliminate them in the order requested.
return factorGraph->eliminateSequential(*varsAsOrdering, function); return factorGraph->eliminateSequential(*varsAsOrdering, function);
@ -291,7 +301,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree( EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Eliminate& function, OptionalVariableIndex variableIndex) const const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
if(!variableIndex) { if(!variableIndex) {
@ -301,10 +311,10 @@ namespace gtsam {
} else { } else {
// No ordering was provided for the marginalized variables, so order them using constrained // No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD. // COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0); bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != 0);
const KeyVector* variablesOrOrdering = const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
unmarginalizedAreOrdered ? ? get_pointer(std::get<const OrderingConstRef>(variables))
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables); : get_pointer(std::get<const KeyVectorConstRef>(variables));
Ordering totalOrdering = Ordering totalOrdering =
Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
@ -315,7 +325,7 @@ namespace gtsam {
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings // 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<class FACTORGRAPH> template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree( EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Ordering& marginalizedVariableOrdering, const Ordering& marginalizedVariableOrdering,
const Eliminate& function, OptionalVariableIndex variableIndex) const const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
@ -338,8 +348,9 @@ namespace gtsam {
const auto [bayesTree, factorGraph] = const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables)) if(std::get_if<const OrderingConstRef>(&variables))
{ {
const Ordering* varsAsOrdering = get_pointer(std::get<const OrderingConstRef>(variables));
// An ordering was also provided for the unmarginalized variables, so we can also // An ordering was also provided for the unmarginalized variables, so we can also
// eliminate them in the order requested. // eliminate them in the order requested.
return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);

View File

@ -22,12 +22,19 @@
#include <cstddef> #include <cstddef>
#include <functional> #include <functional>
#include <optional> #include <optional>
#include <boost/variant.hpp> #include <variant>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
namespace gtsam { namespace gtsam {
// Creating an alias for the variant type since it is verbose
template <typename T>
using ref_wrap = std::reference_wrapper<T>;
using OrderingConstRef = std::reference_wrapper<const Ordering>;
using KeyVectorConstRef = std::reference_wrapper<const KeyVector>;
using OrderingKeyVectorVariant =
std::variant<const OrderingConstRef, const KeyVectorConstRef>;
/// Traits class for eliminateable factor graphs, specifies the types that result from /// 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 /// 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 * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */ * provided one will be computed. */
std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet( std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const; OptionalVariableIndex variableIndex = {}) const;
@ -240,7 +247,7 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */ * provided one will be computed. */
std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet( std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Ordering& marginalizedVariableOrdering, const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const; OptionalVariableIndex variableIndex = {}) const;
@ -255,7 +262,7 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */ * provided one will be computed. */
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree( std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const; OptionalVariableIndex variableIndex = {}) const;
@ -270,7 +277,7 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */ * provided one will be computed. */
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree( std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables, OrderingKeyVectorVariant variables,
const Ordering& marginalizedVariableOrdering, const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const; OptionalVariableIndex variableIndex = {}) const;

View File

@ -38,7 +38,7 @@ namespace gtsam {
Ordering lastKeyAsOrdering; Ordering lastKeyAsOrdering;
lastKeyAsOrdering += lastKey; lastKeyAsOrdering += lastKey;
const GaussianConditional::shared_ptr marginal = const GaussianConditional::shared_ptr marginal =
linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front(); linearFactorGraph.marginalMultifrontalBayesNet(std::cref(lastKeyAsOrdering))->front();
// Extract the current estimate of x1,P1 // Extract the current estimate of x1,P1
VectorValues result = marginal->solve(VectorValues()); VectorValues result = marginal->solve(VectorValues());

View File

@ -32,6 +32,7 @@
#include <limits> #include <limits>
#include <string> #include <string>
#include <utility> #include <utility>
#include <variant>
namespace gtsam { namespace gtsam {
@ -313,13 +314,14 @@ struct GTSAM_EXPORT UpdateImpl {
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys; KeySet relinKeys;
for (const ISAM2::sharedClique& root : roots) { for (const ISAM2::sharedClique& root : roots) {
if (relinearizeThreshold.type() == typeid(double)) if (std::holds_alternative<double>(relinearizeThreshold)) {
CheckRelinearizationRecursiveDouble( CheckRelinearizationRecursiveDouble(
boost::get<double>(relinearizeThreshold), delta, root, &relinKeys); std::get<double>(relinearizeThreshold), delta, root, &relinKeys);
else if (relinearizeThreshold.type() == typeid(FastMap<char, Vector>)) } else if (std::holds_alternative<FastMap<char, Vector>>(relinearizeThreshold)) {
CheckRelinearizationRecursiveMap( CheckRelinearizationRecursiveMap(
boost::get<FastMap<char, Vector> >(relinearizeThreshold), delta, std::get<FastMap<char, Vector> >(relinearizeThreshold), delta,
root, &relinKeys); root, &relinKeys);
}
} }
return relinKeys; return relinKeys;
} }
@ -340,13 +342,13 @@ struct GTSAM_EXPORT UpdateImpl {
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys; KeySet relinKeys;
if (const double* threshold = boost::get<double>(&relinearizeThreshold)) { if (const double* threshold = std::get_if<double>(&relinearizeThreshold)) {
for (const VectorValues::KeyValuePair& key_delta : delta) { for (const VectorValues::KeyValuePair& key_delta : delta) {
double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>(); double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
} }
} else if (const FastMap<char, Vector>* thresholds = } else if (const FastMap<char, Vector>* thresholds =
boost::get<FastMap<char, Vector> >(&relinearizeThreshold)) { std::get_if<FastMap<char, Vector> >(&relinearizeThreshold)) {
for (const VectorValues::KeyValuePair& key_delta : delta) { for (const VectorValues::KeyValuePair& key_delta : delta) {
const Vector& threshold = const Vector& threshold =
thresholds->find(Symbol(key_delta.first).chr())->second; thresholds->find(Symbol(key_delta.first).chr())->second;

View File

@ -28,6 +28,7 @@
#include <algorithm> #include <algorithm>
#include <map> #include <map>
#include <utility> #include <utility>
#include <variant>
using namespace std; using namespace std;
@ -38,16 +39,18 @@ template class BayesTree<ISAM2Clique>;
/* ************************************************************************* */ /* ************************************************************************* */
ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) {
if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) if (std::holds_alternative<ISAM2DoglegParams>(params_.optimizationParams)) {
doglegDelta_ = doglegDelta_ =
boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta; std::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
ISAM2::ISAM2() : update_count_(0) { ISAM2::ISAM2() : update_count_(0) {
if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) if (std::holds_alternative<ISAM2DoglegParams>(params_.optimizationParams)) {
doglegDelta_ = doglegDelta_ =
boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta; std::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -702,10 +705,10 @@ void ISAM2::marginalizeLeaves(
// Marked const but actually changes mutable delta // Marked const but actually changes mutable delta
void ISAM2::updateDelta(bool forceFullSolve) const { void ISAM2::updateDelta(bool forceFullSolve) const {
gttic(updateDelta); gttic(updateDelta);
if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { if (std::holds_alternative<ISAM2GaussNewtonParams>(params_.optimizationParams)) {
// If using Gauss-Newton, update with wildfireThreshold // If using Gauss-Newton, update with wildfireThreshold
const ISAM2GaussNewtonParams& gaussNewtonParams = const ISAM2GaussNewtonParams& gaussNewtonParams =
boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams); std::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
const double effectiveWildfireThreshold = const double effectiveWildfireThreshold =
forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold;
gttic(Wildfire_update); gttic(Wildfire_update);
@ -713,11 +716,10 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
effectiveWildfireThreshold, &delta_); effectiveWildfireThreshold, &delta_);
deltaReplacedMask_.clear(); deltaReplacedMask_.clear();
gttoc(Wildfire_update); gttoc(Wildfire_update);
} else if (std::holds_alternative<ISAM2DoglegParams>(params_.optimizationParams)) {
} else if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
// If using Dogleg, do a Dogleg step // If using Dogleg, do a Dogleg step
const ISAM2DoglegParams& doglegParams = const ISAM2DoglegParams& doglegParams =
boost::get<ISAM2DoglegParams>(params_.optimizationParams); std::get<ISAM2DoglegParams>(params_.optimizationParams);
const double effectiveWildfireThreshold = const double effectiveWildfireThreshold =
forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; forceFullSolve ? 0.0 : doglegParams.wildfireThreshold;

View File

@ -23,6 +23,7 @@
#include <gtsam/nonlinear/DoglegOptimizerImpl.h> #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <string> #include <string>
#include <variant>
namespace gtsam { namespace gtsam {
@ -133,10 +134,10 @@ struct GTSAM_EXPORT ISAM2DoglegParams {
typedef FastMap<char, Vector> ISAM2ThresholdMap; typedef FastMap<char, Vector> ISAM2ThresholdMap;
typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
struct GTSAM_EXPORT ISAM2Params { struct GTSAM_EXPORT ISAM2Params {
typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams> typedef std::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
OptimizationParams; ///< Either ISAM2GaussNewtonParams or OptimizationParams; ///< Either ISAM2GaussNewtonParams or
///< ISAM2DoglegParams ///< ISAM2DoglegParams
typedef boost::variant<double, FastMap<char, Vector> > typedef std::variant<double, FastMap<char, Vector> >
RelinearizationThreshold; ///< Either a constant relinearization RelinearizationThreshold; ///< Either a constant relinearization
///< threshold or a per-variable-type set of ///< threshold or a per-variable-type set of
///< thresholds ///< thresholds
@ -254,20 +255,21 @@ struct GTSAM_EXPORT ISAM2Params {
cout << str << "\n"; cout << str << "\n";
static const std::string kStr("optimizationParams: "); static const std::string kStr("optimizationParams: ");
if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) if (std::holds_alternative<ISAM2GaussNewtonParams>(optimizationParams)) {
boost::get<ISAM2GaussNewtonParams>(optimizationParams).print(); std::get<ISAM2GaussNewtonParams>(optimizationParams).print();
else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) } else if (std::holds_alternative<ISAM2DoglegParams>(optimizationParams)) {
boost::get<ISAM2DoglegParams>(optimizationParams).print(kStr); std::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
else } else {
cout << kStr << "{unknown type}\n"; cout << kStr << "{unknown type}\n";
}
cout << "relinearizeThreshold: "; cout << "relinearizeThreshold: ";
if (relinearizeThreshold.type() == typeid(double)) { if (std::holds_alternative<double>(relinearizeThreshold)) {
cout << boost::get<double>(relinearizeThreshold) << "\n"; cout << std::get<double>(relinearizeThreshold) << "\n";
} else { } else {
cout << "{mapped}\n"; cout << "{mapped}\n";
for (const ISAM2ThresholdMapValue& value : for (const ISAM2ThresholdMapValue& value :
boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) { std::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
cout << " '" << value.first cout << " '" << value.first
<< "' -> [" << value.second.transpose() << " ]\n"; << "' -> [" << value.second.transpose() << " ]\n";
} }

View File

@ -129,8 +129,9 @@ TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional( SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional(
1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3)); 1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3));
auto ordering = Ordering{0,1,2,3};
SymbolicBayesNet actual1 = SymbolicBayesNet actual1 =
*simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3}); *simpleTestGraph2.marginalMultifrontalBayesNet(std::cref(ordering));
EXPECT(assert_equal(expectedBayesNet, actual1)); EXPECT(assert_equal(expectedBayesNet, actual1));
} }

View File

@ -468,7 +468,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
{ {
// Create a set of optimizer parameters // Create a set of optimizer parameters
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -594,7 +594,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 )
{ {
// Create a set of optimizer parameters // Create a set of optimizer parameters
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -641,7 +641,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_2 )
{ {
// Create a set of optimizer parameters // Create a set of optimizer parameters
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -711,7 +711,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 )
{ {
// Create a set of optimizer parameters // Create a set of optimizer parameters
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -798,7 +798,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 )
{ {
// Create a set of optimizer parameters // Create a set of optimizer parameters
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -893,7 +893,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
{ {
// Create a set of optimizer parameters // Create a set of optimizer parameters
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -1182,7 +1182,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
{ {
// Create a set of optimizer parameters // Create a set of optimizer parameters
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -1241,7 +1241,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
// we try removing the last factor // we try removing the last factor
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -1300,7 +1300,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
// we try removing the first factor // we try removing the first factor
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
@ -1357,7 +1357,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
// we try removing the last factor // we try removing the last factor
ISAM2Params parameters; ISAM2Params parameters;
parameters.relinearizeThreshold = 0; parameters.relinearizeThreshold = 0.;
// ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the // 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) // default value for that is 10 (if you set that to zero the code will crash)
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;