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

Fix to allow eliminating variable that doesn't exist using FactorGraph eliminate(One)

release/4.3a0
Richard Roberts 2012-11-26 19:21:09 +00:00
parent 8ca71f833d
commit f142758ec8
1 changed files with 23 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/inference/FactorGraph-inl.h
View File

@ -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<FACTOR> remainingGraph;
remainingGraph.reserve(this->size() - involvedFactors.size() + 1);