boost::variant -> std::variant
parent
6160759f13
commit
a77b5bc1d7
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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());
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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";
|
||||||
}
|
}
|
||||||
|
|
|
@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue