diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index c62af258a..4d8b0a1a4 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -34,7 +34,6 @@ #include #include #include -#include namespace gtsam { diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 3bddbb635..467f7bfe3 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -29,21 +29,22 @@ using namespace std; namespace gtsam { + +/* ************************************************************************* */ +// Create an index formatter that looks up the Key in an inverse ordering, then +// formats the key using the provided key formatter, used in saveGraph. +struct OrderingIndexFormatter { + Ordering::InvertedMap inverseOrdering; + const KeyFormatter& keyFormatter; + OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : + inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {} + string operator()(Index index) { + return keyFormatter(inverseOrdering.at(index)); + } +}; + /* ************************************************************************* */ void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const { - - // Create an index formatter that looks up the Key in an inverse ordering, then - // formats the key using the provided key formatter. - struct OrderingIndexFormatter { - Ordering::InvertedMap inverseOrdering; - const KeyFormatter& keyFormatter; - OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : - inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {} - string operator()(Index index) { - return keyFormatter(inverseOrdering.at(index)); - } - }; - isam_.saveGraph(s, OrderingIndexFormatter(ordering_, keyFormatter)); }