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

Hybrid Mixture error calculation

release/4.3a0
Varun Agrawal 2022-10-26 10:14:09 -04:00
parent 05d4d9187d
commit 64744b057e
2 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -24,11 +24,13 @@
#include <gtsam/discrete/DiscreteKey.h> #include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridGaussianFactor.h> #include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam { namespace gtsam {
class GaussianFactorGraph; class GaussianFactorGraph;
// Needed for wrapper.
using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>; using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
/** /**
@ -125,6 +127,22 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
*/ */
Sum add(const Sum &sum) const; Sum add(const Sum &sum) const;
/**
* @brief Compute error of the GaussianMixtureFactor as a tree.
*
* @param continuousVals The continuous VectorValues.
* @return DecisionTree<Key, double> A decision tree with corresponding keys
* as the factor but leaf values as the error.
*/
DecisionTree<Key, double> error(const VectorValues &c) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [c](const GaussianFactor::shared_ptr &factor) {
return factor->error(c);
};
DecisionTree<Key, double> errorTree(factors_, errorFunc);
return errorTree;
}
/// Add MixtureFactor to a Sum, syntactic sugar. /// Add MixtureFactor to a Sum, syntactic sugar.
friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) { friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) {
sum = factor.add(sum); sum = factor.add(sum);

16
gtsam/hybrid/MixtureFactor.h
View File

@ -121,6 +121,22 @@ class MixtureFactor : public HybridFactor {
~MixtureFactor() = default; ~MixtureFactor() = default;
/**
* @brief Compute error of the MixtureFactor as a tree.
*
* @param continuousVals The continuous values for which to compute the error.
* @return DecisionTree<Key, double> A decision tree with corresponding keys
* as the factor but leaf values as the error.
*/
DecisionTree<Key, double> error(const Values& continuousVals) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [continuousVals](const sharedFactor& factor) {
return factor->error(continuousVals);
};
DecisionTree<Key, double> errorTree(factors_, errorFunc);
return errorTree;
}
/** /**
* @brief Compute error of factor given both continuous and discrete values. * @brief Compute error of factor given both continuous and discrete values.
* *