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

refactored and documented SumFrontals

release/4.3a0
Frank Dellaert 2023-01-01 11:22:44 -05:00
parent 92e2a39c26
commit fa76d53f16
2 changed files with 22 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -65,7 +65,7 @@ static GaussianMixtureFactor::Sum &addGaussian(
if (sum.empty()) {
GaussianFactorGraph result;
result.push_back(factor);
sum = GaussianMixtureFactor::Sum(
return GaussianMixtureFactor::Sum(
GaussianMixtureFactor::GraphAndConstant(result, 0.0));
} else {
@ -75,12 +75,19 @@ static GaussianMixtureFactor::Sum &addGaussian(
result.push_back(factor);
return GaussianMixtureFactor::GraphAndConstant(result, graph_z.constant);
};
sum = sum.apply(add);
return sum.apply(add);
}
return sum;
}
/* ************************************************************************ */
// TODO(dellaert): At the time I though "Sum" was a good name, but coming back
// to it after a while I think "SumFrontals" is better.it's a terrible name.
// Also, the implementation is inconsistent. I think we should just have a
// virtual method in HybridFactor that adds to the "Sum" object, like
// addGaussian. Finally, we need to document why deferredFactors need to be
// added last, which I would undo if possible.
// Implementation-wise, it's probably more efficient to first collect the
// discrete keys, and then loop over all assignments to populate a vector.
GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const {
// sum out frontals, this is the factor on the separator
gttic(sum);
@ -89,8 +96,8 @@ GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const {
std::vector<GaussianFactor::shared_ptr> deferredFactors;
for (auto &f : factors_) {
// TODO(dellaert): just use a virtual method defined in HybridFactor.
if (f->isHybrid()) {
// TODO(dellaert): just use a virtual method defined in HybridFactor.
if (auto gm = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
sum = gm->add(sum);
}

15
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -18,10 +18,10 @@
#pragma once
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#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>
@ -122,9 +122,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @name Adding factors.
/// @{
using Base::reserve;
using Base::add;
using Base::push_back;
using Base::reserve;
/// Add a Jacobian factor to the factor graph.
void add(JacobianFactor&& factor);
@ -236,8 +236,15 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
*/
const Ordering getHybridOrdering() const;
/// Compute a DecisionTree<Key, GaussianFactorGraph> with the marginal for
/// each discrete assignment.
/**
* @brief Create a decision tree of factor graphs out of this hybrid factor
* graph.
*
* For example, if there are two mixture factors, one with a discrete key A
* and one with a discrete key B, then the decision tree will have two levels,
* one for A and one for B. The leaves of the tree will be the Gaussian
* factors that have only continuous keys.
*/
GaussianMixtureFactor::Sum SumFrontals() const;
/// @}