diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 276ee4da0..f454a6af4 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -17,13 +17,13 @@ namespace gtsam { /** - * @brief The main ISAM2 class that is exposed to gtsam users. + * @brief The main ISAM2 class that is exposed to gtsam users, see ISAM2 for usage. * * This is a thin wrapper around an ISAM2 class templated on * GaussianConditional, and the values on which that GaussianISAM2 is * templated. * - * @tparam VALUES The LieValues or TupleValues\Emph{N} to contain the + * @tparam VALUES The LieValues or TupleValues\Emph{N} that contains the * variables. */ template diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 99991ed50..9f879bdbb 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -807,6 +807,7 @@ void ISAM2::update( tic(8,"recalculate"); // 8. Redo top of Bayes tree + boost::shared_ptr > replacedKeys; if(markedKeys.size() > 0 || newKeys.size() > 0) replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors); toc(8,"recalculate"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 40cd083a8..14a65fa5f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -29,80 +29,114 @@ namespace gtsam { -//typedef std::vector CachedFactors; - +/** + * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. + * + * The typical cycle of using this class to create an instance using the default + * constructor, then add measurements and variables as they arrive using the update() + * method. At any time, calculateEstimate() may be called to obtain the current + * estimate of all variables. + */ template class ISAM2: public BayesTree { protected: - // current linearization point + /** The current linearization point */ VALUES theta_; - // VariableIndex lets us look up factors by involved variable and keeps track of dimensions + /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ VariableIndex variableIndex_; - // the linear solution, an update to the estimate in theta + /** The linear delta from the last linear solution, an update to the estimate in theta */ VectorValues deltaUnpermuted_; - // The residual permutation through which the deltaUnpermuted_ is - // referenced. Permuting the VectorVALUES is slow, so for performance the - // permutation is applied at access time instead of to the VectorVALUES - // itself. + /** @brief The permutation through which the deltaUnpermuted_ is + * referenced. + * + * Permuting Vector entries would be slow, so for performance we + * instead maintain this permutation through which we access the linear delta + * indirectly + */ Permuted delta_; - // for keeping all original nonlinear factors + /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; - // The "ordering" allows converting Symbols to Index (integer) keys. We - // keep it up to date as we add and reorder variables. + /** @brief The current elimination ordering Symbols to Index (integer) keys. + * + * We keep it up to date as we add and reorder variables. + */ Ordering ordering_; - // cached intermediate results for restarting computation in the middle - // CachedFactors cached_; - +private: #ifndef NDEBUG std::vector lastRelinVariables_; #endif -public: - - typedef BayesTree Base; - typedef ISAM2 This; - - /** Create an empty Bayes Tree */ - ISAM2(); - - // /** Create a Bayes Tree from a Bayes Net */ - // ISAM2(const NonlinearFactorGraph& fg, const Ordering& ordering, const VALUES& config); - - /** Destructor */ - virtual ~ISAM2() {} - - typedef typename BayesTree::sharedClique sharedClique; - typedef typename BayesTree::Cliques Cliques; typedef JacobianFactor CacheFactor; +public: + + typedef BayesTree Base; ///< The BayesTree base class + typedef ISAM2 This; ///< This class + + /** Create an empty ISAM2 instance */ + ISAM2(); + + typedef typename BayesTree::sharedClique sharedClique; ///< Shared pointer to a clique + typedef typename BayesTree::Cliques Cliques; ///< List of Clique typedef from base class + /** - * ISAM2. + * Add new factors, updating the solution and relinearizing as needed. + * + * Add new measurements, and optionally new variables, to the current system. + * This runs a full step of the ISAM2 algorithm, relinearizing and updating + * the solution as needed, according to the wildfire and relinearize + * thresholds. + * + * @param newFactors The new factors to be added to the system + * @param newTheta Initialization points for new variables to be added to the system. + * You must include here all new variables occuring in newFactors (which were not already + * in the system). There must not be any variables here that do not occur in newFactors, + * and additionally, variables that were already in the system must not be included here. + * @param wildfire_threshold The threshold below which the linear solution delta is not + * updated. + * @param relinearize_threshold The threshold on the linear delta below which a variable + * will not be relinearized. + * @param relinearize */ void update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true, bool force_relinearize = false); - // needed to create initial estimates + /** Access the current linearization point const VALUES& getLinearizationPoint() const {return theta_;} - // estimate based on incomplete delta (threshold!) + /** Compute an estimate from the incomplete linear delta computed during the last update. + * This delta is incomplete because it was not updated below wildfire_threshold. + */ VALUES calculateEstimate() const; - // estimate based on full delta (note that this is based on the current linearization point) + /// @name Public members for non-typical usage + //@{ + + /** Internal implementation functions */ + struct Impl { + static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); + }; + + /** Compute an estimate using a complete delta computed by a full back-substitution. + */ VALUES calculateBestEstimate() const; + /** Access the current delta, computed during the last call to update */ const Permuted& getDelta() const { return delta_; } + /** Access the set of nonlinear factors */ const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + /** Access the current ordering */ const Ordering& getOrdering() const { return ordering_; } size_t lastAffectedVariableCount; @@ -112,7 +146,7 @@ public: size_t lastBacksubVariableCount; size_t lastNnzTop; - boost::shared_ptr > replacedKeys; + //@} private: @@ -124,12 +158,6 @@ private: // void linear_update(const GaussianFactorGraph& newFactors); void find_all(sharedClique clique, FastSet& keys, const std::vector& marked); // helper function -public: - - struct Impl { - static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); - }; - }; // ISAM2 } /// namespace gtsam