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

optional ordering argument for HybridNonlinearISAM::update

release/4.3a0
Varun Agrawal 2022-09-16 10:47:01 -04:00
parent dcad55c032
commit 5765926122
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/hybrid/HybridNonlinearISAM.cpp
View File

@ -33,7 +33,9 @@ void HybridNonlinearISAM::saveGraph(const string& s,
/* ************************************************************************* */
void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues) {
const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering) {
if (newFactors.size() > 0) {
// Reorder and relinearize every reorderInterval updates
if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
@ -51,7 +53,8 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
newFactors.linearize(linPoint_);
// Update ISAM
isam_.update(*linearizedNewFactors, boost::none, eliminationFunction_);
isam_.update(*linearizedNewFactors, maxNrLeaves, ordering,
eliminationFunction_);
}
}
@ -66,7 +69,7 @@ void HybridNonlinearISAM::reorder_relinearize() {
// Just recreate the whole BayesTree
// TODO: allow for constrained ordering here
// TODO: decouple relinearization and reordering to avoid
isam_.update(*factors_.linearize(newLinPoint), boost::none,
isam_.update(*factors_.linearize(newLinPoint), boost::none, boost::none,
eliminationFunction_);
// Update linearization point

4
gtsam/hybrid/HybridNonlinearISAM.h
View File

@ -118,7 +118,9 @@ class GTSAM_EXPORT HybridNonlinearISAM {
/** Add new factors along with their initial linearization points */
void update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues);
const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none);
/** Relinearization and reordering of variables */
void reorder_relinearize();