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

HybridISAM docs

release/4.3a0
Varun Agrawal 2022-05-27 16:23:35 -04:00
parent 30c8e1dfa7
commit 4ee4b37f2f
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/hybrid/HybridISAM.cpp
View File

@ -36,6 +36,7 @@ HybridISAM::HybridISAM() {}
/* ************************************************************************* */ /* ************************************************************************* */
HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
/* ************************************************************************* */
void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans, HybridBayesTree::Cliques* orphans,
const HybridBayesTree::Eliminate& function) { const HybridBayesTree::Eliminate& function) {
@ -79,8 +80,6 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
true); true);
ordering.print("ORD");
// eliminate all factors (top, added, orphans) into a new Bayes tree // eliminate all factors (top, added, orphans) into a new Bayes tree
auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
@ -90,6 +89,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
} }
/* ************************************************************************* */
void HybridISAM::update(const HybridFactorGraph& newFactors, void HybridISAM::update(const HybridFactorGraph& newFactors,
const HybridBayesTree::Eliminate& function) { const HybridBayesTree::Eliminate& function) {
Cliques orphans; Cliques orphans;

9
gtsam/hybrid/HybridISAM.h
View File

@ -43,11 +43,20 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
/// @} /// @}
private:
/// Internal method that performs the ISAM update.
void updateInternal( void updateInternal(
const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans,
const HybridBayesTree::Eliminate& function = const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate); HybridBayesTree::EliminationTraitsType::DefaultEliminate);
public:
/**
* @brief Perform update step with new factors.
*
* @param newFactors Factor graph of new factors to add and eliminate.
* @param function Elimination function.
*/
void update(const HybridFactorGraph& newFactors, void update(const HybridFactorGraph& newFactors,
const HybridBayesTree::Eliminate& function = const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate); HybridBayesTree::EliminationTraitsType::DefaultEliminate);