From f142758ec8ac02080036e19ac8ddbea6451bb6ca Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 26 Nov 2012 19:21:09 +0000 Subject: [PATCH] Fix to allow eliminating variable that doesn't exist using FactorGraph eliminate(One) --- gtsam/inference/FactorGraph-inl.h | 37 +++++++++++++++++++------------ 1 file changed, 23 insertions(+), 14 deletions(-) diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 0701a791f..783ed1db3 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -177,22 +177,31 @@ namespace gtsam { *std::max_element(factor->begin(), factor->end())); } - // Now permute the variables to be eliminated to the front of the ordering - Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1); - Permutation toFrontInverse = *toFront.inverse(); - BOOST_FOREACH(const sharedFactor& factor, involvedFactors) { - factor->permuteWithInverse(toFrontInverse); + sharedConditional conditional; + sharedFactor remainingFactor; + if(involvedFactors.size() > 0) { + // Now permute the variables to be eliminated to the front of the ordering + Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1); + Permutation toFrontInverse = *toFront.inverse(); + BOOST_FOREACH(const sharedFactor& factor, involvedFactors) { + factor->permuteWithInverse(toFrontInverse); + } + + // Eliminate into conditional and remaining factor + EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size()); + conditional = eliminated.first; + remainingFactor = eliminated.second; + + // Undo the permutation + conditional->permuteWithInverse(toFront); + remainingFactor->permuteWithInverse(toFront); + } else { + // Eliminate 0 variables + EliminationResult eliminated = eliminateFcn(involvedFactors, 0); + conditional = eliminated.first; + remainingFactor = eliminated.second; } - // Eliminate into conditional and remaining factor - EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size()); - sharedConditional conditional = eliminated.first; - sharedFactor remainingFactor = eliminated.second; - - // Undo the permutation - conditional->permuteWithInverse(toFront); - remainingFactor->permuteWithInverse(toFront); - // Build the remaining graph, without the removed factors FactorGraph remainingGraph; remainingGraph.reserve(this->size() - involvedFactors.size() + 1);