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

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

@ -177,6 +177,9 @@ namespace gtsam {
*std::max_element(factor->begin(), factor->end())); *std::max_element(factor->begin(), factor->end()));
} }
sharedConditional conditional;
sharedFactor remainingFactor;
if(involvedFactors.size() > 0) {
// Now permute the variables to be eliminated to the front of the ordering // Now permute the variables to be eliminated to the front of the ordering
Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1); Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1);
Permutation toFrontInverse = *toFront.inverse(); Permutation toFrontInverse = *toFront.inverse();
@ -186,12 +189,18 @@ namespace gtsam {
// Eliminate into conditional and remaining factor // Eliminate into conditional and remaining factor
EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size()); EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size());
sharedConditional conditional = eliminated.first; conditional = eliminated.first;
sharedFactor remainingFactor = eliminated.second; remainingFactor = eliminated.second;
// Undo the permutation // Undo the permutation
conditional->permuteWithInverse(toFront); conditional->permuteWithInverse(toFront);
remainingFactor->permuteWithInverse(toFront); remainingFactor->permuteWithInverse(toFront);
} else {
// Eliminate 0 variables
EliminationResult eliminated = eliminateFcn(involvedFactors, 0);
conditional = eliminated.first;
remainingFactor = eliminated.second;
}
// Build the remaining graph, without the removed factors // Build the remaining graph, without the removed factors
FactorGraph<FACTOR> remainingGraph; FactorGraph<FACTOR> remainingGraph;