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