diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 8b30cb963..fdfc2bc0d 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -70,8 +70,8 @@ template<> struct EliminationTraits /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { - return Ordering::Colamd(*variableIndex); + const VariableIndex& variableIndex) { + return Ordering::Colamd(variableIndex); } }; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 9724fccc5..432bf7c07 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -87,7 +87,7 @@ struct EliminationTraits { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { + const VariableIndex&) { return HybridOrdering(graph); } }; diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 086127f16..b6534df70 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -102,7 +102,7 @@ void HybridGaussianISAM::updateInternal( // eliminate all factors (top, added, orphans) into a new Bayes tree HybridBayesTree::shared_ptr bayesTree = - factors.eliminateMultifrontal(elimination_ordering, function, index); + factors.eliminateMultifrontal(elimination_ordering, function, &index); if (maxNrLeaves) { bayesTree->prune(*maxNrLeaves); diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index bebce14cd..a0847dbdd 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -36,7 +36,7 @@ namespace gtsam { // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(orderingType, function, computedVariableIndex); + return eliminateSequential(orderingType, function, &computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to have a @@ -52,7 +52,7 @@ namespace gtsam { return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), variableIndex); + asDerived(), *variableIndex); return eliminateSequential(computedOrdering, function, variableIndex); } } @@ -68,7 +68,7 @@ namespace gtsam { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex); + return eliminateSequential(ordering, function, &computedVariableIndex); } else { gttic(eliminateSequential); // Do elimination @@ -99,7 +99,7 @@ namespace gtsam { // creating ordering. VariableIndex computedVariableIndex(asDerived()); return eliminateMultifrontal(orderingType, function, - computedVariableIndex); + &computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in @@ -115,7 +115,7 @@ namespace gtsam { return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), variableIndex); + asDerived(), *variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex); } } @@ -131,7 +131,7 @@ namespace gtsam { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex); + return eliminateMultifrontal(ordering, function, &computedVariableIndex); } else { gttic(eliminateMultifrontal); // Do elimination with given ordering @@ -162,7 +162,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialSequential(ordering, function, computedVariableIndex); + return eliminatePartialSequential(ordering, function, &computedVariableIndex); } } @@ -183,7 +183,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialSequential(variables, function, computedVariableIndex); + return eliminatePartialSequential(variables, function, &computedVariableIndex); } } @@ -202,7 +202,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialMultifrontal(ordering, function, computedVariableIndex); + return eliminatePartialMultifrontal(ordering, function, &computedVariableIndex); } } @@ -223,7 +223,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialMultifrontal(variables, function, computedVariableIndex); + return eliminatePartialMultifrontal(variables, function, &computedVariableIndex); } } @@ -237,7 +237,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); - return marginalMultifrontalBayesNet(variables, function, index); + return marginalMultifrontalBayesNet(variables, function, &index); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -255,7 +255,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(marginalVarsOrdering, marginalizationOrdering, function, variableIndex); } } @@ -270,7 +270,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); - return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); + return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, &index); } else { gttic(marginalMultifrontalBayesNet); // An ordering was provided for the marginalized variables, so we can first eliminate them @@ -278,7 +278,7 @@ namespace gtsam { boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); if(const Ordering* varsAsOrdering = boost::get(&variables)) { @@ -304,7 +304,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginalMultifrontalBayesTree(variables, function, computedVariableIndex); + return marginalMultifrontalBayesTree(variables, function, &computedVariableIndex); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -322,7 +322,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(marginalVarsOrdering, marginalizationOrdering, function, variableIndex); } } @@ -337,7 +337,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); + return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, &computedVariableIndex); } else { gttic(marginalMultifrontalBayesTree); // An ordering was provided for the marginalized variables, so we can first eliminate them @@ -345,7 +345,7 @@ namespace gtsam { boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); if(const Ordering* varsAsOrdering = boost::get(&variables)) { @@ -377,13 +377,13 @@ namespace gtsam { Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); // Eliminate and return the remaining factor graph - return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second; + return eliminatePartialMultifrontal(marginalizationOrdering, function, variableIndex).second; } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginal(variables, function, computedVariableIndex); + return marginal(variables, function, &computedVariableIndex); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index c6cf56a11..1ee990807 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -89,7 +89,7 @@ namespace gtsam { typedef std::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions - typedef boost::optional OptionalVariableIndex; + typedef const VariableIndex* OptionalVariableIndex; /// Typedef for an optional ordering type typedef std::optional OptionalOrderingType; diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 39d840906..5dd06702c 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -49,7 +49,7 @@ void ISAM::updateInternal(const FactorGraphType& newFactors, KeyVector(newFactorKeys.begin(), newFactorKeys.end())); // eliminate all factors (top, added, orphans) into a new Bayes tree - auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); + auto bayesTree = factors.eliminateMultifrontal(ordering, function, &index); // Re-add into Bayes tree data structures this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index fb5e1ea36..8b268558a 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -57,8 +57,8 @@ namespace gtsam { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { - return Ordering::Colamd(*variableIndex); + const VariableIndex& variableIndex) { + return Ordering::Colamd(variableIndex); } }; diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 1d4292cbf..ecb7c222b 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -49,8 +49,8 @@ namespace gtsam { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { - return Ordering::Colamd(*variableIndex); + const VariableIndex& variableIndex) { + return Ordering::Colamd(variableIndex); } };