-
Notifications
You must be signed in to change notification settings - Fork 767
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #1290 from borglab/hybrid/improvements
- Loading branch information
Showing
9 changed files
with
879 additions
and
22 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,111 @@ | ||
| /* ---------------------------------------------------------------------------- | ||
| * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||
| * Atlanta, Georgia 30332-0415 | ||
| * All Rights Reserved | ||
| * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||
| * See LICENSE for the license information | ||
| * -------------------------------------------------------------------------- */ | ||
|
|
||
| /** | ||
| * @file HybridNonlinearISAM.cpp | ||
| * @date Sep 12, 2022 | ||
| * @author Varun Agrawal | ||
| */ | ||
|
|
||
| #include <gtsam/hybrid/HybridGaussianFactorGraph.h> | ||
| #include <gtsam/hybrid/HybridNonlinearISAM.h> | ||
| #include <gtsam/inference/Ordering.h> | ||
|
|
||
| #include <iostream> | ||
|
|
||
| using namespace std; | ||
|
|
||
| namespace gtsam { | ||
|
|
||
| /* ************************************************************************* */ | ||
| void HybridNonlinearISAM::saveGraph(const string& s, | ||
| const KeyFormatter& keyFormatter) const { | ||
| isam_.saveGraph(s, keyFormatter); | ||
| } | ||
|
|
||
| /* ************************************************************************* */ | ||
| void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, | ||
| const Values& initialValues) { | ||
| if (newFactors.size() > 0) { | ||
| // Reorder and relinearize every reorderInterval updates | ||
| if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { | ||
| reorder_relinearize(); | ||
| reorderCounter_ = 0; | ||
| } | ||
|
|
||
| factors_.push_back(newFactors); | ||
|
|
||
| // Linearize new factors and insert them | ||
| // TODO: optimize for whole config? | ||
| linPoint_.insert(initialValues); | ||
|
|
||
| boost::shared_ptr<HybridGaussianFactorGraph> linearizedNewFactors = | ||
| newFactors.linearize(linPoint_); | ||
|
|
||
| // Update ISAM | ||
| isam_.update(*linearizedNewFactors, boost::none, eliminationFunction_); | ||
| } | ||
| } | ||
|
|
||
| /* ************************************************************************* */ | ||
| void HybridNonlinearISAM::reorder_relinearize() { | ||
| if (factors_.size() > 0) { | ||
| // Obtain the new linearization point | ||
| const Values newLinPoint = estimate(); | ||
|
|
||
| isam_.clear(); | ||
|
|
||
| // 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, | ||
| eliminationFunction_); | ||
|
|
||
| // Update linearization point | ||
| linPoint_ = newLinPoint; | ||
| } | ||
| } | ||
|
|
||
| /* ************************************************************************* */ | ||
| Values HybridNonlinearISAM::estimate() { | ||
| Values result; | ||
| if (isam_.size() > 0) { | ||
| HybridValues values = isam_.optimize(); | ||
| assignment_ = values.discrete(); | ||
| return linPoint_.retract(values.continuous()); | ||
| } else { | ||
| return linPoint_; | ||
| } | ||
| } | ||
|
|
||
| // /* ************************************************************************* | ||
| // */ Matrix HybridNonlinearISAM::marginalCovariance(Key key) const { | ||
| // return isam_.marginalCovariance(key); | ||
| // } | ||
|
|
||
| /* ************************************************************************* */ | ||
| void HybridNonlinearISAM::print(const string& s, | ||
| const KeyFormatter& keyFormatter) const { | ||
| cout << s << "ReorderInterval: " << reorderInterval_ | ||
| << " Current Count: " << reorderCounter_ << endl; | ||
| isam_.print("HybridGaussianISAM:\n"); | ||
| linPoint_.print("Linearization Point:\n", keyFormatter); | ||
| factors_.print("Nonlinear Graph:\n", keyFormatter); | ||
| } | ||
|
|
||
| /* ************************************************************************* */ | ||
| void HybridNonlinearISAM::printStats() const { | ||
| isam_.getCliqueData().getStats().print(); | ||
| } | ||
|
|
||
| /* ************************************************************************* */ | ||
|
|
||
| } // namespace gtsam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,132 @@ | ||
| /* ---------------------------------------------------------------------------- | ||
| * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||
| * Atlanta, Georgia 30332-0415 | ||
| * All Rights Reserved | ||
| * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||
| * See LICENSE for the license information | ||
| * -------------------------------------------------------------------------- */ | ||
|
|
||
| /** | ||
| * @file HybridNonlinearISAM.h | ||
| * @date Sep 12, 2022 | ||
| * @author Varun Agrawal | ||
| */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <gtsam/hybrid/HybridGaussianISAM.h> | ||
| #include <gtsam/hybrid/HybridNonlinearFactorGraph.h> | ||
|
|
||
| namespace gtsam { | ||
| /** | ||
| * Wrapper class to manage ISAM in a nonlinear context | ||
| */ | ||
| class GTSAM_EXPORT HybridNonlinearISAM { | ||
| protected: | ||
| /** The internal iSAM object */ | ||
| gtsam::HybridGaussianISAM isam_; | ||
|
|
||
| /** The current linearization point */ | ||
| Values linPoint_; | ||
|
|
||
| /// The discrete assignment | ||
| DiscreteValues assignment_; | ||
|
|
||
| /** The original factors, used when relinearizing */ | ||
| HybridNonlinearFactorGraph factors_; | ||
|
|
||
| /** The reordering interval and counter */ | ||
| int reorderInterval_; | ||
| int reorderCounter_; | ||
|
|
||
| /** The elimination function */ | ||
| HybridGaussianFactorGraph::Eliminate eliminationFunction_; | ||
|
|
||
| public: | ||
| /// @name Standard Constructors | ||
| /// @{ | ||
|
|
||
| /** | ||
| * Periodically reorder and relinearize | ||
| * @param reorderInterval is the number of updates between reorderings, | ||
| * 0 never reorders (and is dangerous for memory consumption) | ||
| * 1 (default) reorders every time, in worse case is batch every update | ||
| * typical values are 50 or 100 | ||
| */ | ||
| HybridNonlinearISAM( | ||
| int reorderInterval = 1, | ||
| const HybridGaussianFactorGraph::Eliminate& eliminationFunction = | ||
| HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate) | ||
| : reorderInterval_(reorderInterval), | ||
| reorderCounter_(0), | ||
| eliminationFunction_(eliminationFunction) {} | ||
|
|
||
| /// @} | ||
| /// @name Standard Interface | ||
| /// @{ | ||
|
|
||
| /** Return the current solution estimate */ | ||
| Values estimate(); | ||
|
|
||
| // /** find the marginal covariance for a single variable */ | ||
| // Matrix marginalCovariance(Key key) const; | ||
|
|
||
| // access | ||
|
|
||
| /** access the underlying bayes tree */ | ||
| const HybridGaussianISAM& bayesTree() const { return isam_; } | ||
|
|
||
| /** | ||
| * @brief Prune the underlying Bayes tree. | ||
| * | ||
| * @param root The root key in the discrete conditional decision tree. | ||
| * @param maxNumberLeaves | ||
| */ | ||
| void prune(const Key& root, const size_t maxNumberLeaves) { | ||
| isam_.prune(root, maxNumberLeaves); | ||
| } | ||
|
|
||
| /** Return the current linearization point */ | ||
| const Values& getLinearizationPoint() const { return linPoint_; } | ||
|
|
||
| /** Return the current discrete assignment */ | ||
| const DiscreteValues& getAssignment() const { return assignment_; } | ||
|
|
||
| /** get underlying nonlinear graph */ | ||
| const HybridNonlinearFactorGraph& getFactorsUnsafe() const { | ||
| return factors_; | ||
| } | ||
|
|
||
| /** get counters */ | ||
| int reorderInterval() const { return reorderInterval_; } ///< TODO: comment | ||
| int reorderCounter() const { return reorderCounter_; } ///< TODO: comment | ||
|
|
||
| /** prints out all contents of the system */ | ||
| void print(const std::string& s = "", | ||
| const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; | ||
|
|
||
| /** prints out clique statistics */ | ||
| void printStats() const; | ||
|
|
||
| /** saves the Tree to a text file in GraphViz format */ | ||
| void saveGraph(const std::string& s, | ||
| const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; | ||
|
|
||
| /// @} | ||
| /// @name Advanced Interface | ||
| /// @{ | ||
|
|
||
| /** Add new factors along with their initial linearization points */ | ||
| void update(const HybridNonlinearFactorGraph& newFactors, | ||
| const Values& initialValues); | ||
|
|
||
| /** Relinearization and reordering of variables */ | ||
| void reorder_relinearize(); | ||
|
|
||
| /// @} | ||
| }; | ||
|
|
||
| } // namespace gtsam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.