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

simplify the error addition

release/4.3a0
Varun Agrawal 2022-12-31 12:33:37 +01:00
parent 02d7b877a3
commit 878eeb5d73
1 changed files with 3 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -235,7 +235,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
gttoc_(hybrid_eliminate); gttoc_(hybrid_eliminate);
#endif #endif
//TODO(Varun) The normalizing constant has to be computed correctly // TODO(Varun) The normalizing constant has to be computed correctly
return {conditional_factor.first, {conditional_factor.second, 0.0}}; return {conditional_factor.first, {conditional_factor.second, 0.0}};
}; };
@ -461,15 +461,8 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
// If factor is hybrid, select based on assignment. // If factor is hybrid, select based on assignment.
GaussianMixtureFactor::shared_ptr gaussianMixture = GaussianMixtureFactor::shared_ptr gaussianMixture =
boost::static_pointer_cast<GaussianMixtureFactor>(factors_.at(idx)); boost::static_pointer_cast<GaussianMixtureFactor>(factors_.at(idx));
// Compute factor error. // Compute factor error and add it.
factor_error = gaussianMixture->error(continuousValues); error_tree = error_tree + gaussianMixture->error(continuousValues);
// If first factor, assign error, else add it.
if (idx == 0) {
error_tree = factor_error;
} else {
error_tree = error_tree + factor_error;
}
} else if (factors_.at(idx)->isContinuous()) { } else if (factors_.at(idx)->isContinuous()) {
// If continuous only, get the (double) error // If continuous only, get the (double) error