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

custom ProductAndNormalize for TableFactor

release/4.3a0
Varun Agrawal 2025-01-01 14:02:15 -05:00
parent ec5d87e1a5
commit 2a5833bf6a
2 changed files with 13 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

@ -24,7 +24,6 @@
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteJunctionTree.h> #include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/discrete/DiscreteKey.h> #include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteTableConditional.h>
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/discrete/TableFactor.h> #include <gtsam/discrete/TableFactor.h>
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
@ -256,14 +255,8 @@ static TableFactor::shared_ptr DiscreteFactorFromErrors(
return std::make_shared<TableFactor>(discreteKeys, potentials); return std::make_shared<TableFactor>(discreteKeys, potentials);
} }
/** /* ************************************************************************ */
* @brief Multiply all the `factors` and normalize the TableFactor TableProductAndNormalize(const DiscreteFactorGraph &factors) {
* product to prevent underflow.
*
* @param factors The factors to multiply as a DiscreteFactorGraph.
* @return TableFactor
*/
static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
// PRODUCT: multiply all factors // PRODUCT: multiply all factors
#if GTSAM_HYBRID_TIMING #if GTSAM_HYBRID_TIMING
gttic_(DiscreteProduct); gttic_(DiscreteProduct);
@ -352,7 +345,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
#endif #endif
/**** NOTE: This does sum-product. ****/ /**** NOTE: This does sum-product. ****/
// Get product factor // Get product factor
TableFactor product = ProductAndNormalize(dfg); TableFactor product = TableProductAndNormalize(dfg);
#if GTSAM_HYBRID_TIMING #if GTSAM_HYBRID_TIMING
gttic_(EliminateDiscreteSum); gttic_(EliminateDiscreteSum);

10
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -20,6 +20,7 @@
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteKey.h> #include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteTableConditional.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h> #include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h> #include <gtsam/hybrid/HybridGaussianFactor.h>
@ -270,4 +271,13 @@ template <>
struct traits<HybridGaussianFactorGraph> struct traits<HybridGaussianFactorGraph>
: public Testable<HybridGaussianFactorGraph> {}; : public Testable<HybridGaussianFactorGraph> {};
/**
* @brief Multiply all the `factors` and normalize the
* product to prevent underflow.
*
* @param factors The factors to multiply as a DiscreteFactorGraph.
* @return TableFactor
*/
TableFactor TableProductAndNormalize(const DiscreteFactorGraph& factors);
} // namespace gtsam } // namespace gtsam