From 4f7151c320996afe55a67cd3b9f2806619bc1b03 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 12:06:04 -0800 Subject: [PATCH] use ref wrapper in eliminateable factor graph --- gtsam/hybrid/HybridGaussianISAM.cpp | 2 +- .../inference/EliminateableFactorGraph-inst.h | 52 +++++++++---------- gtsam/inference/EliminateableFactorGraph.h | 3 +- gtsam/inference/ISAM-inst.h | 2 +- 4 files changed, 30 insertions(+), 29 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index b6534df70..dc7746209 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, std::cref(index)); if (maxNrLeaves) { bayesTree->prune(*maxNrLeaves); diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index a0847dbdd..b7aa52bab 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, std::cref(computedVariableIndex)); } else { // Compute an ordering and call this function again. We are guaranteed to have a @@ -45,14 +45,14 @@ namespace gtsam { Ordering computedOrdering = Ordering::Metis(asDerived()); return eliminateSequential(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::COLAMD) { - Ordering computedOrdering = Ordering::Colamd(*variableIndex); + Ordering computedOrdering = Ordering::Colamd((*variableIndex).get()); return eliminateSequential(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::NATURAL) { Ordering computedOrdering = Ordering::Natural(asDerived()); return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), *variableIndex); + asDerived(), (*variableIndex).get()); return eliminateSequential(computedOrdering, function, variableIndex); } } @@ -68,11 +68,11 @@ 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, std::cref(computedVariableIndex)); } else { gttic(eliminateSequential); // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); boost::shared_ptr bayesNet; boost::shared_ptr factorGraph; boost::tie(bayesNet,factorGraph) = etree.eliminate(function); @@ -99,7 +99,7 @@ namespace gtsam { // creating ordering. VariableIndex computedVariableIndex(asDerived()); return eliminateMultifrontal(orderingType, function, - &computedVariableIndex); + std::cref(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 @@ -108,14 +108,14 @@ namespace gtsam { Ordering computedOrdering = Ordering::Metis(asDerived()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::COLAMD) { - Ordering computedOrdering = Ordering::Colamd(*variableIndex); + Ordering computedOrdering = Ordering::Colamd((*variableIndex).get()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::NATURAL) { Ordering computedOrdering = Ordering::Natural(asDerived()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), *variableIndex); + asDerived(), (*variableIndex).get()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } } @@ -131,11 +131,11 @@ 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, std::cref(computedVariableIndex)); } else { gttic(eliminateMultifrontal); // Do elimination with given ordering - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); JunctionTreeType junctionTree(etree); boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; @@ -157,12 +157,12 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); return etree.eliminate(function); } 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, std::cref(computedVariableIndex)); } } @@ -175,7 +175,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Compute full ordering - Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -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, std::cref(computedVariableIndex)); } } @@ -196,13 +196,13 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); JunctionTreeType junctionTree(etree); return junctionTree.eliminate(function); } 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, std::cref(computedVariableIndex)); } } @@ -215,7 +215,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Compute full ordering - Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -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, std::cref(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, std::cref(index)); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -247,7 +247,7 @@ namespace gtsam { boost::get(&variables) : boost::get(&variables); Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -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 @@ -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, std::cref(computedVariableIndex)); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -314,7 +314,7 @@ namespace gtsam { boost::get(&variables) : boost::get(&variables); Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -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, std::cref(computedVariableIndex)); } else { gttic(marginalMultifrontalBayesTree); // An ordering was provided for the marginalized variables, so we can first eliminate them @@ -371,7 +371,7 @@ namespace gtsam { if(variableIndex) { // Compute a total ordering for all variables - Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables); + Ordering totalOrdering = Ordering::ColamdConstrainedLast((*variableIndex).get(), variables); // Split out the part for the marginalized variables Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); @@ -383,7 +383,7 @@ namespace gtsam { { // 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, std::cref(computedVariableIndex)); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 599baa7da..8cdf9abca 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -90,7 +90,8 @@ namespace gtsam { typedef std::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions - typedef const VariableIndex* OptionalVariableIndex; + /// It is an optional to a constant reference + typedef std::optional> 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 5dd06702c..6026e052a 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, std::cref(index)); // Re-add into Bayes tree data structures this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),