12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/inference/BayesTree-inst.h
View File

@ -26,6 +26,7 @@
#include <gtsam/base/timing.h>
#include <fstream>
#include <functional>
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);
}
/* ************************************************************************* */

5
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -20,6 +20,8 @@
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/base/timing.h>
#include <functional>
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;
}
}

43
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -21,6 +21,15 @@
#include <gtsam/inference/EliminateableFactorGraph.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 {
/* ************************************************************************* */
@ -226,7 +235,7 @@ namespace gtsam {
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> 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<const Ordering&>(&variables) != 0);
const KeyVector* variablesOrOrdering =
unmarginalizedAreOrdered ?
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != nullptr);
const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
? get_pointer(std::get<const OrderingConstRef>(variables))
: get_pointer(std::get<const KeyVectorConstRef>(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<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> 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<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
// eliminate them in the order requested.
return factorGraph->eliminateSequential(*varsAsOrdering, function);
@ -291,7 +301,7 @@ namespace gtsam {
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> 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<const Ordering&>(&variables) != 0);
const KeyVector* variablesOrOrdering =
unmarginalizedAreOrdered ?
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != 0);
const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
? get_pointer(std::get<const OrderingConstRef>(variables))
: get_pointer(std::get<const KeyVectorConstRef>(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<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> 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<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
// eliminate them in the order requested.
return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);

17
gtsam/inference/EliminateableFactorGraph.h
View File

@ -22,12 +22,19 @@
#include <cstddef>
#include <functional>
#include <optional>
#include <boost/variant.hpp>
#include <variant>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
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
/// 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<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> 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<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables,
OrderingKeyVectorVariant variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const;

2
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -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());

14
gtsam/nonlinear/ISAM2-impl.h
View File

@ -32,6 +32,7 @@
#include <limits>
#include <string>
#include <utility>
#include <variant>
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<double>(relinearizeThreshold)) {
CheckRelinearizationRecursiveDouble(
boost::get<double>(relinearizeThreshold), delta, root, &relinKeys);
else if (relinearizeThreshold.type() == typeid(FastMap<char, Vector>))
std::get<double>(relinearizeThreshold), delta, root, &relinKeys);
} else if (std::holds_alternative<FastMap<char, Vector>>(relinearizeThreshold)) {
CheckRelinearizationRecursiveMap(
boost::get<FastMap<char, Vector> >(relinearizeThreshold), delta,
std::get<FastMap<char, Vector> >(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<double>(&relinearizeThreshold)) {
if (const double* threshold = std::get_if<double>(&relinearizeThreshold)) {
for (const VectorValues::KeyValuePair& key_delta : delta) {
double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
}
} 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) {
const Vector& threshold =
thresholds->find(Symbol(key_delta.first).chr())->second;

20
gtsam/nonlinear/ISAM2.cpp
View File

@ -28,6 +28,7 @@
#include <algorithm>
#include <map>
#include <utility>
#include <variant>
using namespace std;
@ -38,16 +39,18 @@ template class BayesTree<ISAM2Clique>;
/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) {
if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
if (std::holds_alternative<ISAM2DoglegParams>(params_.optimizationParams)) {
doglegDelta_ =
boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
std::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
}
/* ************************************************************************* */
ISAM2::ISAM2() : update_count_(0) {
if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
if (std::holds_alternative<ISAM2DoglegParams>(params_.optimizationParams)) {
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
void ISAM2::updateDelta(bool forceFullSolve) const {
gttic(updateDelta);
if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
if (std::holds_alternative<ISAM2GaussNewtonParams>(params_.optimizationParams)) {
// If using Gauss-Newton, update with wildfireThreshold
const ISAM2GaussNewtonParams& gaussNewtonParams =
boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
std::get<ISAM2GaussNewtonParams>(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<ISAM2DoglegParams>(params_.optimizationParams)) {
// If using Dogleg, do a Dogleg step
const ISAM2DoglegParams& doglegParams =
boost::get<ISAM2DoglegParams>(params_.optimizationParams);
std::get<ISAM2DoglegParams>(params_.optimizationParams);
const double effectiveWildfireThreshold =
forceFullSolve ? 0.0 : doglegParams.wildfireThreshold;

22
gtsam/nonlinear/ISAM2Params.h
View File

@ -23,6 +23,7 @@
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <string>
#include <variant>
namespace gtsam {
@ -133,10 +134,10 @@ struct GTSAM_EXPORT ISAM2DoglegParams {
typedef FastMap<char, Vector> ISAM2ThresholdMap;
typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
struct GTSAM_EXPORT ISAM2Params {
typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
typedef std::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
OptimizationParams; ///< Either ISAM2GaussNewtonParams or
///< ISAM2DoglegParams
typedef boost::variant<double, FastMap<char, Vector> >
typedef std::variant<double, FastMap<char, Vector> >
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<ISAM2GaussNewtonParams>(optimizationParams).print();
else if (optimizationParams.type() == typeid(ISAM2DoglegParams))
boost::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
else
if (std::holds_alternative<ISAM2GaussNewtonParams>(optimizationParams)) {
std::get<ISAM2GaussNewtonParams>(optimizationParams).print();
} else if (std::holds_alternative<ISAM2DoglegParams>(optimizationParams)) {
std::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
} else {
cout << kStr << "{unknown type}\n";
}
cout << "relinearizeThreshold: ";
if (relinearizeThreshold.type() == typeid(double)) {
cout << boost::get<double>(relinearizeThreshold) << "\n";
if (std::holds_alternative<double>(relinearizeThreshold)) {
cout << std::get<double>(relinearizeThreshold) << "\n";
} else {
cout << "{mapped}\n";
for (const ISAM2ThresholdMapValue& value :
boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
std::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
cout << " '" << value.first
<< "' -> [" << value.second.transpose() << " ]\n";
}

3
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -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));
}

20
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -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;