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

Made SumFrontals a method to test

release/4.3a0
Frank Dellaert 2022-12-31 18:27:24 -05:00
parent c8008cbb7c
commit b463386514
2 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -81,15 +81,14 @@ static GaussianMixtureFactor::Sum &addGaussian(
}
/* ************************************************************************ */
static GaussianMixtureFactor::Sum sumFrontals(
const HybridGaussianFactorGraph &factors) {
GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const {
// sum out frontals, this is the factor on the separator
gttic(sum);
GaussianMixtureFactor::Sum sum;
std::vector<GaussianFactor::shared_ptr> deferredFactors;
for (auto &f : factors) {
for (auto &f : factors_) {
if (f->isHybrid()) {
// TODO(dellaert): just use a virtual method defined in HybridFactor.
if (auto gm = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
@ -194,7 +193,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
// Collect all the frontal factors to create Gaussian factor graphs
// indexed on the discrete keys.
GaussianMixtureFactor::Sum sum = sumFrontals(factors);
GaussianMixtureFactor::Sum sum = factors.SumFrontals();
// If a tree leaf contains nullptr,
// convert that leaf to an empty GaussianFactorGraph.

32
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -21,6 +21,7 @@
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
@ -118,14 +119,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
: Base(graph) {}
/// @}
/// @name Adding factors.
/// @{
using Base::empty;
using Base::reserve;
using Base::size;
using Base::operator[];
using Base::add;
using Base::push_back;
using Base::resize;
/// Add a Jacobian factor to the factor graph.
void add(JacobianFactor&& factor);
@ -172,6 +171,25 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
}
}
/// @}
/// @name Testable
/// @{
// TODO(dellaert): customize print and equals.
// void print(const std::string& s = "HybridGaussianFactorGraph",
// const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
// override;
// bool equals(const This& fg, double tol = 1e-9) const override;
/// @}
/// @name Standard Interface
/// @{
using Base::empty;
using Base::size;
using Base::operator[];
using Base::resize;
/**
* @brief Compute error for each discrete assignment,
* and return as a tree.
@ -217,6 +235,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
* @return const Ordering
*/
const Ordering getHybridOrdering() const;
/// Compute a DecisionTree<Key, GaussianFactorGraph> with the marginal for
/// each discrete assignment.
GaussianMixtureFactor::Sum SumFrontals() const;
/// @}
};
} // namespace gtsam