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

add back PotentiallyPrunedComponentError as an inline function

release/4.3a0
Varun Agrawal 2024-10-08 23:21:46 -04:00
parent cd3e0f37fb
commit 4ae5596200
1 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -175,13 +175,19 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const {
}}};
}
/* *******************************************************************************/
inline static double PotentiallyPrunedComponentError(
const GaussianFactorValuePair& pair, const VectorValues& continuousValues) {
return pair.first ? pair.first->error(continuousValues) + pair.second
: std::numeric_limits<double>::infinity();
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
const VectorValues& continuousValues) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) {
return pair.first ? pair.first->error(continuousValues) + pair.second
: std::numeric_limits<double>::infinity();
return PotentiallyPrunedComponentError(pair, continuousValues);
};
DecisionTree<Key, double> error_tree(factors_, errorFunc);
return error_tree;
@ -191,8 +197,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
double HybridGaussianFactor::error(const HybridValues& values) const {
// Directly index to get the component, no need to build the whole tree.
const GaussianFactorValuePair pair = factors_(values.discrete());
return pair.first ? pair.first->error(values.continuous()) + pair.second
: std::numeric_limits<double>::infinity();
return PotentiallyPrunedComponentError(pair, values.continuous());
}
} // namespace gtsam