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, std::optional<size_t> maxNrLeaves,
const std::optional<Ordering> given_ordering) { const std::optional<Ordering> given_ordering) {
HybridGaussianFactorGraph linearizedFactors = *newFactors.linearize(initial);
const KeySet originalNewFactorKeys = newFactors.keys(); const KeySet originalNewFactorKeys = newFactors.keys();
#ifdef DEBUG_SMOOTHER #ifdef DEBUG_SMOOTHER
std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size()
@ -108,7 +110,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
HybridGaussianFactorGraph updatedGraph; HybridGaussianFactorGraph updatedGraph;
// Add the necessary conditionals from the previous timestep(s). // Add the necessary conditionals from the previous timestep(s).
std::tie(updatedGraph, hybridBayesNet_) = std::tie(updatedGraph, hybridBayesNet_) =
addConditionals(newFactors, hybridBayesNet_); addConditionals(linearizedFactors, hybridBayesNet_);
#ifdef DEBUG_SMOOTHER #ifdef DEBUG_SMOOTHER
// print size of newFactors, updatedGraph, hybridBayesNet_ // print size of newFactors, updatedGraph, hybridBayesNet_
std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl; 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/discrete/DiscreteFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <optional> #include <optional>
@ -26,8 +27,9 @@ namespace gtsam {
class GTSAM_EXPORT HybridSmoother { class GTSAM_EXPORT HybridSmoother {
private: private:
HybridBayesNet hybridBayesNet_; HybridNonlinearFactorGraph allFactors_;
HybridBayesNet hybridBayesNet_;
/// The threshold above which we make a decision about a mode. /// The threshold above which we make a decision about a mode.
std::optional<double> marginalThreshold_; std::optional<double> marginalThreshold_;
DiscreteValues fixedValues_; DiscreteValues fixedValues_;
@ -73,12 +75,12 @@ class GTSAM_EXPORT HybridSmoother {
* @param graph The new factors, should be linear only * @param graph The new factors, should be linear only
* @param maxNrLeaves The maximum number of leaves in the new discrete factor, * @param maxNrLeaves The maximum number of leaves in the new discrete factor,
* if applicable * if applicable
* @param given_ordering The (optional) ordering for elimination, only * @param givenOrdering The (optional) ordering for elimination, only
* continuous variables are allowed * continuous variables are allowed
*/ */
void update(const HybridGaussianFactorGraph& graph, void update(const HybridNonlinearFactorGraph& graph, const Values& initial,
std::optional<size_t> maxNrLeaves = {}, std::optional<size_t> maxNrLeaves = {},
const std::optional<Ordering> given_ordering = {}); const std::optional<Ordering> givenOrdering = {});
/** /**
* @brief Get an elimination ordering which eliminates continuous * @brief Get an elimination ordering which eliminates continuous