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

hybrid smoother directly accepts nonlinear factors

release/4.3a0
Varun Agrawal 2025-03-17 16:23:40 -04:00
parent 2dc45bac96
commit 3fed73b993
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/hybrid/HybridSmoother.cpp
View File

@ -96,9 +96,11 @@ void HybridSmoother::removeFixedValues(
}
/* ************************************************************************* */
void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
void HybridSmoother::update(const HybridNonlinearFactorGraph &newFactors,
const Values &initial,
std::optional<size_t> maxNrLeaves,
const std::optional<Ordering> given_ordering) {
HybridGaussianFactorGraph linearizedFactors = *newFactors.linearize(initial);
const KeySet originalNewFactorKeys = newFactors.keys();
#ifdef DEBUG_SMOOTHER
std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size()
@ -108,7 +110,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
HybridGaussianFactorGraph updatedGraph;
// Add the necessary conditionals from the previous timestep(s).
std::tie(updatedGraph, hybridBayesNet_) =
addConditionals(newFactors, hybridBayesNet_);
addConditionals(linearizedFactors, hybridBayesNet_);
#ifdef DEBUG_SMOOTHER
// print size of newFactors, updatedGraph, hybridBayesNet_
std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl;

10
gtsam/hybrid/HybridSmoother.h
View File

@ -19,6 +19,7 @@
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <optional>
@ -26,8 +27,9 @@ namespace gtsam {
class GTSAM_EXPORT HybridSmoother {
private:
HybridBayesNet hybridBayesNet_;
HybridNonlinearFactorGraph allFactors_;
HybridBayesNet hybridBayesNet_;
/// The threshold above which we make a decision about a mode.
std::optional<double> marginalThreshold_;
DiscreteValues fixedValues_;
@ -73,12 +75,12 @@ class GTSAM_EXPORT HybridSmoother {
* @param graph The new factors, should be linear only
* @param maxNrLeaves The maximum number of leaves in the new discrete factor,
* if applicable
* @param given_ordering The (optional) ordering for elimination, only
* @param givenOrdering The (optional) ordering for elimination, only
* continuous variables are allowed
*/
void update(const HybridGaussianFactorGraph& graph,
void update(const HybridNonlinearFactorGraph& graph, const Values& initial,
std::optional<size_t> maxNrLeaves = {},
const std::optional<Ordering> given_ordering = {});
const std::optional<Ordering> givenOrdering = {});
/**
* @brief Get an elimination ordering which eliminates continuous