From 5a358489dc91daeaafd6b6dcb6f84670d479e104 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 13 Oct 2019 23:54:22 -0400 Subject: [PATCH 1/5] Some low hanging fruit changing `func(..., boost::optional)` to overloaded `func(...)` and `func(..., const Class &)` not all done Also, although it currently passes all tests, I think some more tests may be needed... --- gtsam/base/TestableAssertions.h | 8 +- gtsam/inference/EliminateableFactorGraph.h | 4 +- gtsam/linear/GaussianBayesNet.cpp | 21 +-- gtsam/linear/GaussianBayesNet.h | 9 +- gtsam/linear/GaussianFactorGraph.cpp | 45 +++++- gtsam/linear/GaussianFactorGraph.h | 47 +++++- gtsam/linear/HessianFactor.cpp | 4 +- gtsam/linear/HessianFactor.h | 6 +- gtsam/linear/IterativeSolver.cpp | 21 ++- gtsam/linear/IterativeSolver.h | 16 +- gtsam/linear/JacobianFactor.cpp | 172 +++++++++++++++------ gtsam/linear/JacobianFactor.h | 36 ++++- gtsam/linear/RegularHessianFactor.h | 8 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 8 +- 14 files changed, 304 insertions(+), 101 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 1a31aa2848..a698f8f984 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -71,12 +71,8 @@ bool assert_equal(const V& expected, const boost::optional& actual, double to } template -bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { - if (!actual) { - std::cout << "actual is boost::none" << std::endl; - return false; - } - return assert_equal(expected, *actual, tol); +bool assert_equal(const V& expected, double tol = 1e-9) { + return false; } /** diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 8084aa75b3..249c594b60 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -89,10 +89,10 @@ namespace gtsam { typedef boost::function Eliminate; /// Typedef for an optional ordering as an argument to elimination functions - typedef boost::optional OptionalOrdering; + typedef const boost::optional& OptionalOrdering; /// Typedef for an optional variable index as an argument to elimination functions - typedef boost::optional OptionalVariableIndex; + typedef const boost::optional& OptionalVariableIndex; /// Typedef for an optional ordering type typedef boost::optional OptionalOrderingType; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index e9938ceb65..04094d5934 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -156,16 +156,17 @@ namespace gtsam { } /* ************************************************************************* */ - pair GaussianBayesNet::matrix(boost::optional ordering) const { - if (ordering) { - // Convert to a GaussianFactorGraph and use its machinery - GaussianFactorGraph factorGraph(*this); - return factorGraph.jacobian(ordering); - } else { - // recursively call with default ordering - const auto defaultOrdering = this->ordering(); - return matrix(defaultOrdering); - } + pair GaussianBayesNet::matrix(const Ordering& ordering) const { + // Convert to a GaussianFactorGraph and use its machinery + GaussianFactorGraph factorGraph(*this); + return factorGraph.jacobian(ordering); + } + + /* ************************************************************************* */ + pair GaussianBayesNet::matrix() const { + // recursively call with default ordering + const auto defaultOrdering = this->ordering(); + return matrix(defaultOrdering); } ///* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 669c8a9643..9da7a16091 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -92,7 +92,14 @@ namespace gtsam { * Will return upper-triangular matrix only when using 'ordering' above. * In case Bayes net is incomplete zero columns are added to the end. */ - std::pair matrix(boost::optional ordering = boost::none) const; + std::pair matrix(const Ordering& ordering) const; + + /** + * Return (dense) upper-triangular matrix representation + * Will return upper-triangular matrix only when using 'ordering' above. + * In case Bayes net is incomplete zero columns are added to the end. + */ + std::pair matrix() const; /** * Optimize along the gradient direction, with a closed-form computation to perform the line diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 9281c7e7a2..fe0a9b2dfc 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -190,33 +190,62 @@ namespace gtsam { /* ************************************************************************* */ Matrix GaussianFactorGraph::augmentedJacobian( - boost::optional optionalOrdering) const { + const Ordering& ordering) const { // combine all factors - JacobianFactor combined(*this, optionalOrdering); + JacobianFactor combined(*this, ordering); + return combined.augmentedJacobian(); + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::augmentedJacobian() const { + // combine all factors + JacobianFactor combined(*this); return combined.augmentedJacobian(); } /* ************************************************************************* */ pair GaussianFactorGraph::jacobian( - boost::optional optionalOrdering) const { - Matrix augmented = augmentedJacobian(optionalOrdering); + const Ordering& ordering) const { + Matrix augmented = augmentedJacobian(ordering); + return make_pair(augmented.leftCols(augmented.cols() - 1), + augmented.col(augmented.cols() - 1)); + } + + /* ************************************************************************* */ + pair GaussianFactorGraph::jacobian() const { + Matrix augmented = augmentedJacobian(); return make_pair(augmented.leftCols(augmented.cols() - 1), augmented.col(augmented.cols() - 1)); } /* ************************************************************************* */ Matrix GaussianFactorGraph::augmentedHessian( - boost::optional optionalOrdering) const { + const Ordering& ordering) const { + // combine all factors and get upper-triangular part of Hessian + Scatter scatter(*this, ordering); + HessianFactor combined(*this, scatter); + return combined.info().selfadjointView();; + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::augmentedHessian() const { // combine all factors and get upper-triangular part of Hessian - Scatter scatter(*this, optionalOrdering); + Scatter scatter(*this); HessianFactor combined(*this, scatter); return combined.info().selfadjointView();; } /* ************************************************************************* */ pair GaussianFactorGraph::hessian( - boost::optional optionalOrdering) const { - Matrix augmented = augmentedHessian(optionalOrdering); + const Ordering& ordering) const { + Matrix augmented = augmentedHessian(ordering); + size_t n = augmented.rows() - 1; + return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1)); + } + + /* ************************************************************************* */ + pair GaussianFactorGraph::hessian() const { + Matrix augmented = augmentedHessian(); size_t n = augmented.rows() - 1; return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1)); } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 49cba482d9..8bdf4e4754 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -201,7 +201,16 @@ namespace gtsam { * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. */ - Matrix augmentedJacobian(boost::optional optionalOrdering = boost::none) const; + Matrix augmentedJacobian(const Ordering& ordering) const; + + /** + * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ + * Jacobian matrix, augmented with b with the noise models baked + * into A and b. The negative log-likelihood is + * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. + */ + Matrix augmentedJacobian() const; /** * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, @@ -210,7 +219,29 @@ namespace gtsam { * GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::sparseJacobian. */ - std::pair jacobian(boost::optional optionalOrdering = boost::none) const; + std::pair jacobian(const Ordering& ordering) const; + + /** + * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, + * with the noise models baked into A and b. The negative log-likelihood + * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::augmentedJacobian and + * GaussianFactorGraph::sparseJacobian. + */ + std::pair jacobian() const; + + /** + * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian + * matrix, augmented with the information vector \f$ \eta \f$. The + * augmented Hessian is + \f[ \left[ \begin{array}{ccc} + \Lambda & \eta \\ + \eta^T & c + \end{array} \right] \f] + and the negative log-likelihood is + \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. + */ + Matrix augmentedHessian(const Ordering& ordering) const; /** * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian @@ -223,7 +254,15 @@ namespace gtsam { and the negative log-likelihood is \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. */ - Matrix augmentedHessian(boost::optional optionalOrdering = boost::none) const; + Matrix augmentedHessian() const; + + /** + * Return the dense Hessian \f$ \Lambda \f$ and information vector + * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood + * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also + * GaussianFactorGraph::augmentedHessian. + */ + std::pair hessian(const Ordering& ordering) const; /** * Return the dense Hessian \f$ \Lambda \f$ and information vector @@ -231,7 +270,7 @@ namespace gtsam { * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also * GaussianFactorGraph::augmentedHessian. */ - std::pair hessian(boost::optional optionalOrdering = boost::none) const; + std::pair hessian() const; /** Return only the diagonal of the Hessian A'*A, as a VectorValues */ virtual VectorValues hessianDiagonal() const; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c208259b86..cc82e2fa0b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -250,10 +250,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter) { + const Scatter& scatter) { gttic(HessianFactor_MergeConstructor); - Allocate(scatter ? *scatter : Scatter(factors)); + Allocate(scatter); // Form A' * A gttic(update); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index cb9da0f1a4..f0c79d8fbd 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -176,7 +176,11 @@ namespace gtsam { /** Combine a set of factors into a single dense HessianFactor */ explicit HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter = boost::none); + const Scatter& scatter); + + /** Combine a set of factors into a single dense HessianFactor */ + explicit HessianFactor(const GaussianFactorGraph& factors) + : HessianFactor(factors, Scatter(factors)) {} /** Destructor */ virtual ~HessianFactor() {} diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index c7d4e54057..9478f6fbfb 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -84,16 +84,25 @@ string IterativeOptimizationParameters::verbosityTranslator( /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - boost::optional keyInfo, - boost::optional&> lambda) { - return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), - lambda ? *lambda : std::map()); + const KeyInfo &keyInfo, const std::map &lambda) { + return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda) { - return optimize(gfg, keyInfo, lambda, keyInfo.x0()); + const KeyInfo& keyInfo) { + return optimize(gfg, keyInfo, std::map()); +} + +/*****************************************************************************/ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + const std::map& lambda) { + return optimize(gfg, KeyInfo(gfg), lambda); +} + +/*****************************************************************************/ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) { + return optimize(gfg, KeyInfo(gfg), std::map()); } /****************************************************************************/ diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 758d2aec97..84ebe52cbe 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -91,15 +91,21 @@ class IterativeSolver { virtual ~IterativeSolver() { } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - boost::optional = boost::none, - boost::optional&> lambda = boost::none); - /* interface to the nonlinear optimizer, without initial estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda); + /* interface to the nonlinear optimizer, without damping and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo& keyInfo); + + /* interface to the nonlinear optimizer, without metadata and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + const std::map& lambda); + + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg); + /* interface to the nonlinear optimizer that the subclasses have to implement */ virtual VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 06f6b3bed2..9be010ff5e 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -220,60 +220,13 @@ FastVector _convertOrCastToJacobians( } /* ************************************************************************* */ -JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, - boost::optional ordering, - boost::optional p_variableSlots) { - gttic(JacobianFactor_combine_constructor); - - // Compute VariableSlots if one was not provided - // Binds reference, does not copy VariableSlots - const VariableSlots & variableSlots = - p_variableSlots ? p_variableSlots.get() : VariableSlots(graph); +void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph, + const FastVector& orderedSlots) { // Cast or convert to Jacobians FastVector jacobians = _convertOrCastToJacobians( graph); - gttic(Order_slots); - // Order variable slots - we maintain the vector of ordered slots, as well as keep a list - // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then - // be added after all of the ordered variables. - FastVector orderedSlots; - orderedSlots.reserve(variableSlots.size()); - if (ordering) { - // If an ordering is provided, arrange the slots first that ordering - FastList unorderedSlots; - size_t nOrderingSlotsUsed = 0; - orderedSlots.resize(ordering->size()); - FastMap inverseOrdering = ordering->invert(); - for (VariableSlots::const_iterator item = variableSlots.begin(); - item != variableSlots.end(); ++item) { - FastMap::const_iterator orderingPosition = - inverseOrdering.find(item->first); - if (orderingPosition == inverseOrdering.end()) { - unorderedSlots.push_back(item); - } else { - orderedSlots[orderingPosition->second] = item; - ++nOrderingSlotsUsed; - } - } - if (nOrderingSlotsUsed != ordering->size()) - throw std::invalid_argument( - "The ordering provided to the JacobianFactor combine constructor\n" - "contained extra variables that did not appear in the factors to combine."); - // Add the remaining slots - for(VariableSlots::const_iterator item: unorderedSlots) { - orderedSlots.push_back(item); - } - } else { - // If no ordering is provided, arrange the slots as they were, which will be sorted - // numerically since VariableSlots uses a map sorting on Key. - for (VariableSlots::const_iterator item = variableSlots.begin(); - item != variableSlots.end(); ++item) - orderedSlots.push_back(item); - } - gttoc(Order_slots); - // Count dimensions FastVector varDims; DenseIndex m, n; @@ -344,6 +297,127 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, this->setModel(anyConstrained, *sigmas); } +/* ************************************************************************* */ +// Order variable slots - we maintain the vector of ordered slots, as well as keep a list +// 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then +// be added after all of the ordered variables. +FastVector orderedSlotsHelper( + const Ordering& ordering, + const VariableSlots& variableSlots) { + gttic(Order_slots); + + FastVector orderedSlots; + orderedSlots.reserve(variableSlots.size()); + + // If an ordering is provided, arrange the slots first that ordering + FastList unorderedSlots; + size_t nOrderingSlotsUsed = 0; + orderedSlots.resize(ordering.size()); + FastMap inverseOrdering = ordering.invert(); + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) { + FastMap::const_iterator orderingPosition = + inverseOrdering.find(item->first); + if (orderingPosition == inverseOrdering.end()) { + unorderedSlots.push_back(item); + } else { + orderedSlots[orderingPosition->second] = item; + ++nOrderingSlotsUsed; + } + } + if (nOrderingSlotsUsed != ordering.size()) + throw std::invalid_argument( + "The ordering provided to the JacobianFactor combine constructor\n" + "contained extra variables that did not appear in the factors to combine."); + // Add the remaining slots + for(VariableSlots::const_iterator item: unorderedSlots) { + orderedSlots.push_back(item); + } + + gttoc(Order_slots); + + return orderedSlots; +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph) { + gttic(JacobianFactor_combine_constructor); + + // Compute VariableSlots if one was not provided + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = VariableSlots(graph); + + gttic(Order_slots); + // Order variable slots - we maintain the vector of ordered slots, as well as keep a list + // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then + // be added after all of the ordered variables. + FastVector orderedSlots; + orderedSlots.reserve(variableSlots.size()); + + // If no ordering is provided, arrange the slots as they were, which will be sorted + // numerically since VariableSlots uses a map sorting on Key. + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) + orderedSlots.push_back(item); + gttoc(Order_slots); + + JacobianFactorHelper(graph, orderedSlots); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, + const VariableSlots& p_variableSlots) { + gttic(JacobianFactor_combine_constructor); + + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = p_variableSlots; + + gttic(Order_slots); + // Order variable slots - we maintain the vector of ordered slots, as well as keep a list + // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then + // be added after all of the ordered variables. + FastVector orderedSlots; + orderedSlots.reserve(variableSlots.size()); + + // If no ordering is provided, arrange the slots as they were, which will be sorted + // numerically since VariableSlots uses a map sorting on Key. + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) + orderedSlots.push_back(item); + gttoc(Order_slots); + + JacobianFactorHelper(graph, orderedSlots); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, + const Ordering& ordering) { + gttic(JacobianFactor_combine_constructor); + + // Compute VariableSlots if one was not provided + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = VariableSlots(graph); + + // Order variable slots + FastVector orderedSlots = + orderedSlotsHelper(ordering, variableSlots); + + JacobianFactorHelper(graph, orderedSlots); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, + const Ordering& ordering, + const VariableSlots& p_variableSlots) { + gttic(JacobianFactor_combine_constructor); + + // Order variable slots + FastVector orderedSlots = + orderedSlotsHelper(ordering, p_variableSlots); + + JacobianFactorHelper(graph, orderedSlots); +} + /* ************************************************************************* */ void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 36d1e12da6..c8d77c5540 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -147,14 +148,37 @@ namespace gtsam { JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph); + /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * structure computed for \c graph is already available, providing it will reduce the amount of * computation performed. */ explicit JacobianFactor( const GaussianFactorGraph& graph, - boost::optional ordering = boost::none, - boost::optional p_variableSlots = boost::none); + const VariableSlots& p_variableSlots); + + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph, + const Ordering& ordering); + + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph, + const Ordering& ordering, + const VariableSlots& p_variableSlots); /** Virtual destructor */ virtual ~JacobianFactor() {} @@ -356,6 +380,14 @@ namespace gtsam { private: + /** + * Helper function for public constructors: + * Build a dense joint factor from all the factors in a factor graph. Takes in + * ordered variable slots */ + void JacobianFactorHelper( + const GaussianFactorGraph& graph, + const FastVector& orderedSlots); + /** Unsafe Constructor that creates an uninitialized Jacobian of right size * @param keys in some order * @param diemnsions of the variables in same order diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index 1fe7009bc3..a24acfacdb 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -77,11 +77,17 @@ class RegularHessianFactor: public HessianFactor { /// Construct from a GaussianFactorGraph RegularHessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter = boost::none) + const Scatter& scatter) : HessianFactor(factors, scatter) { checkInvariants(); } + /// Construct from a GaussianFactorGraph + RegularHessianFactor(const GaussianFactorGraph& factors) + : HessianFactor(factors) { + checkInvariants(); + } + private: /// Check invariants after construction diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index e1efa20615..b12dcf70a3 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -128,17 +128,17 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point VectorValues delta; - boost::optional optionalOrdering; + Ordering ordering; if (params.ordering) - optionalOrdering.reset(*params.ordering); + ordering = *params.ordering; // Check which solver we are using if (params.isMultifrontal()) { // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.optimize(optionalOrdering, params.getEliminationFunction()); + delta = gfg.optimize(ordering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(optionalOrdering, params.getEliminationFunction(), boost::none, + delta = gfg.eliminateSequential(ordering, params.getEliminationFunction(), boost::none, params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams From 1733f3ac98323cd24c042e0f2cc774e2da2c45dc Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 20 Oct 2019 01:15:20 -0400 Subject: [PATCH 2/5] convert all optional Ordering to function overloads compiles and passes tests, but some potentially code-breaking changes in: Marginals.h - order of arguments had to change since `factorization` has a default value EliminatableFactorGraph.h - marginalMultifrontalBayesNet and marginalMultifrontalBayesTree no longer accept `boost::none` as a placeholder to specify later arguments Notes: EliminateableFactorGraph.h - `orderingType` is no longer needed in function overloads that specify ordering, but I left it for the time being to avoid potential code breaking --- gtsam/inference/BayesTree-inst.h | 4 +- gtsam/inference/BayesTreeCliqueBase-inst.h | 2 +- .../inference/EliminateableFactorGraph-inst.h | 283 ++++++++++-------- gtsam/inference/EliminateableFactorGraph.h | 91 ++++-- gtsam/linear/GaussianFactorGraph.cpp | 9 +- gtsam/linear/GaussianFactorGraph.h | 9 +- gtsam/linear/IterativeSolver.cpp | 21 +- gtsam/linear/IterativeSolver.h | 16 +- gtsam/linear/Scatter.cpp | 34 ++- gtsam/linear/Scatter.h | 14 +- gtsam/nonlinear/Marginals.cpp | 60 +++- gtsam/nonlinear/Marginals.h | 41 ++- gtsam_unstable/discrete/CSP.cpp | 9 +- gtsam_unstable/discrete/CSP.h | 5 +- tests/testGaussianFactorGraphB.cpp | 4 +- 15 files changed, 384 insertions(+), 218 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 4df234004f..639bcbab00 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -262,7 +262,7 @@ namespace gtsam { // Now, marginalize out everything that is not variable j BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet( - Ordering(cref_list_of<1,Key>(j)), boost::none, function); + Ordering(cref_list_of<1,Key>(j)), function); // The Bayes net should contain only one conditional for variable j, so return it return marginalBN.front(); @@ -383,7 +383,7 @@ namespace gtsam { } // now, marginalize out everything that is not variable j1 or j2 - return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), boost::none, function); + return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), function); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 6bcfb434d1..e762786f5e 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -171,7 +171,7 @@ namespace gtsam { // The variables we want to keepSet are exactly the ones in S KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); - cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); + cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); } } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index af2a91257e..a77d96537c 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,13 +28,44 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, + const Eliminate& function, OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check + // for no variable index first so that it's always computed if we need to call COLAMD because + // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex + // before creating ordering. + VariableIndex computedVariableIndex(asDerived()); + return eliminateSequential(function, computedVariableIndex, orderingType); + } + else { + // Compute an ordering and call this function again. We are guaranteed to have a + // VariableIndex already here because we computed one if needed in the previous 'if' block. + if (orderingType == Ordering::METIS) { + Ordering computedOrdering = Ordering::Metis(asDerived()); + return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + } else { + Ordering computedOrdering = Ordering::Colamd(*variableIndex); + return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + } + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::eliminateSequential( + const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { - if(ordering && variableIndex) { + if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + } else { gttic(eliminateSequential); // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, *ordering); + EliminationTreeType etree(asDerived(), *variableIndex, ordering); boost::shared_ptr bayesNet; boost::shared_ptr factorGraph; boost::tie(bayesNet,factorGraph) = etree.eliminate(function); @@ -44,23 +75,32 @@ namespace gtsam { // Return the Bayes net return bayesNet; } - else if(!variableIndex) { + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + const Eliminate& function, OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const + { + if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because - // no Ordering is provided. + // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex + // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + return eliminateMultifrontal(function, computedVariableIndex, orderingType); } - else /*if(!ordering)*/ { - // If no Ordering provided, compute one and call this function again. We are guaranteed to - // have a VariableIndex already here because we computed one if needed in the previous 'else' - // block. + else { + // Compute an ordering and call this function again. We are guaranteed to have a + // VariableIndex already here because we computed one if needed in the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); } } } @@ -69,13 +109,17 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, + const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { - if(ordering && variableIndex) { + if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); + } else { gttic(eliminateMultifrontal); // Do elimination with given ordering - EliminationTreeType etree(asDerived(), *variableIndex, *ordering); + EliminationTreeType etree(asDerived(), *variableIndex, ordering); JunctionTreeType junctionTree(etree); boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; @@ -86,25 +130,6 @@ namespace gtsam { // Return the Bayes tree return bayesTree; } - else if(!variableIndex) { - // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check - // for no variable index first so that it's always computed if we need to call COLAMD because - // no Ordering is provided. - VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); - } - else /*if(!ordering)*/ { - // If no Ordering provided, compute one and call this function again. We are guaranteed to - // have a VariableIndex already here because we computed one if needed in the previous 'else' - // block. - if (orderingType == Ordering::METIS) { - Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); - } else { - Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); - } - } } /* ************************************************************************* */ @@ -191,57 +216,97 @@ namespace gtsam { boost::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { - if(variableIndex) - { - if(marginalizedVariableOrdering) + if(!variableIndex) { + // If no variable index is provided, compute one and call this function again + VariableIndex index(asDerived()); + return marginalMultifrontalBayesNet(variables, function, index); + } else { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const KeyVector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get(&variables); + + Ordering totalOrdering = + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::marginalMultifrontalBayesNet( + boost::variant variables, + const Ordering& marginalizedVariableOrdering, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(!variableIndex) { + // If no variable index is provided, compute one and call this function again + VariableIndex index(asDerived()); + return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); + } else { + gttic(marginalMultifrontalBayesNet); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) { - gttic(marginalMultifrontalBayesNet); - // An ordering was provided for the marginalized variables, so we can first eliminate them - // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; - boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); - - if(const Ordering* varsAsOrdering = boost::get(&variables)) - { - // An ordering was also provided for the unmarginalized variables, so we can also - // eliminate them in the order requested. - return factorGraph->eliminateSequential(*varsAsOrdering, function); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateSequential(boost::none, function); - } + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateSequential(*varsAsOrdering, function); } else { - // No ordering was provided for the marginalized variables, so order them using constrained - // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); - - Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); - - // Split up ordering - const size_t nVars = variablesOrOrdering->size(); - Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); - Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); - - // Call this function again with the computed orderings - return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateSequential(function); } - } else { + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::marginalMultifrontalBayesTree( + boost::variant variables, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(!variableIndex) { // If no variable index is provided, compute one and call this function again - VariableIndex index(asDerived()); - return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); + VariableIndex computedVariableIndex(asDerived()); + return marginalMultifrontalBayesTree(variables, function, computedVariableIndex); + } else { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const KeyVector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get(&variables); + + Ordering totalOrdering = + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); } } @@ -250,57 +315,33 @@ namespace gtsam { boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering, + const Ordering& marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { - if(variableIndex) - { - if(marginalizedVariableOrdering) + if(!variableIndex) { + // If no variable index is provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); + } else { + gttic(marginalMultifrontalBayesTree); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) { - gttic(marginalMultifrontalBayesTree); - // An ordering was provided for the marginalized variables, so we can first eliminate them - // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; - boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); - - if(const Ordering* varsAsOrdering = boost::get(&variables)) - { - // An ordering was also provided for the unmarginalized variables, so we can also - // eliminate them in the order requested. - return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateMultifrontal(boost::none, function); - } + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); } else { - // No ordering was provided for the marginalized variables, so order them using constrained - // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); - - Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); - - // Split up ordering - const size_t nVars = variablesOrOrdering->size(); - Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); - Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); - - // Call this function again with the computed orderings - return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateMultifrontal(function); } - } else { - // If no variable index is provided, compute one and call this function again - VariableIndex computedVariableIndex(asDerived()); - return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 249c594b60..8a216918c3 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -88,9 +88,6 @@ namespace gtsam { /// The function type that does a single dense elimination step on a subgraph. typedef boost::function Eliminate; - /// Typedef for an optional ordering as an argument to elimination functions - typedef const boost::optional& OptionalOrdering; - /// Typedef for an optional variable index as an argument to elimination functions typedef const boost::optional& OptionalVariableIndex; @@ -108,24 +105,39 @@ namespace gtsam { * Example - METIS ordering for elimination * \code * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); + * \endcode + * + * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: + * \code + * VariableIndex varIndex(graph); // Build variable index + * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, varIndex, boost::none); + * \endcode + * */ + boost::shared_ptr eliminateSequential( + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; + + /** Do sequential elimination of all variables to produce a Bayes net. * * Example - Full QR elimination in specified order: * \code - * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, myOrdering); + * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR); * \endcode * - * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: + * Example - Reusing an existing VariableIndex to improve performance: * \code * VariableIndex varIndex(graph); // Build variable index * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index - * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); + * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, boost::none); * \endcode * */ boost::shared_ptr eliminateSequential( - OptionalOrdering ordering = boost::none, + const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalOrderingType orderingType = boost::none) const; // orderingType is not necessary anymore, kept for backwards compatibility /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -136,11 +148,6 @@ namespace gtsam { * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); * \endcode * - * Example - Full QR elimination in specified order: - * \code - * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); - * \endcode - * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index @@ -149,11 +156,25 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateMultifrontal( - OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const; + /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not + * provided, the ordering will be computed using either COLAMD or METIS, dependeing on + * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); + * \endcode + * */ + boost::shared_ptr eliminateMultifrontal( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; // orderingType no longer needed + /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ @@ -194,20 +215,47 @@ namespace gtsam { const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /** Compute the marginal of the requested variables and return the result as a Bayes net. Uses + * COLAMD marginalization ordering by default + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a KeyVector they will be ordered using constrained COLAMD. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant variables, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + /** Compute the marginal of the requested variables and return the result as a Bayes net. * @param variables Determines the variables whose marginal to compute, if provided as an * Ordering they will be ordered in the returned BayesNet as specified, and if provided * as a KeyVector they will be ordered using constrained COLAMD. - * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized - * out, i.e. all variables not in \c variables. If this is boost::none, the ordering - * will be computed with COLAMD. + * @param marginalizedVariableOrdering Ordering for the variables being marginalized out, + * i.e. all variables not in \c variables. * @param function Optional dense elimination function, if not provided the default will be * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering = boost::none, + const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal of the requested variables and return the result as a Bayes tree. Uses + * COLAMD marginalization order by default + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a KeyVector they will be ordered using constrained COLAMD. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesTree( + boost::variant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -215,16 +263,15 @@ namespace gtsam { * @param variables Determines the variables whose marginal to compute, if provided as an * Ordering they will be ordered in the returned BayesNet as specified, and if provided * as a KeyVector they will be ordered using constrained COLAMD. - * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized - * out, i.e. all variables not in \c variables. If this is boost::none, the ordering - * will be computed with COLAMD. + * @param marginalizedVariableOrdering Ordering for the variables being marginalized out, + * i.e. all variables not in \c variables. * @param function Optional dense elimination function, if not provided the default will be * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering = boost::none, + const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index fe0a9b2dfc..faa3f8bd60 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -282,8 +282,13 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const - { + VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const { + gttic(GaussianFactorGraph_optimize); + return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + } + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, const Eliminate& function) const { gttic(GaussianFactorGraph_optimize); return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize(); } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 8bdf4e4754..f24fb602d0 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -282,7 +282,14 @@ namespace gtsam { * the dense elimination function specified in \c function (default EliminatePreferCholesky), * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent * to calling graph.eliminateMultifrontal()->optimize(). */ - VectorValues optimize(OptionalOrdering ordering = boost::none, + VectorValues optimize( + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + + /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using + * the dense elimination function specified in \c function (default EliminatePreferCholesky), + * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent + * to calling graph.eliminateMultifrontal()->optimize(). */ + VectorValues optimize(const Ordering&, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; /** diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 9478f6fbfb..c7d4e54057 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -84,25 +84,16 @@ string IterativeOptimizationParameters::verbosityTranslator( /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda) { - return optimize(gfg, keyInfo, lambda, keyInfo.x0()); -} - -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo& keyInfo) { - return optimize(gfg, keyInfo, std::map()); + boost::optional keyInfo, + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); } /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const std::map& lambda) { - return optimize(gfg, KeyInfo(gfg), lambda); -} - -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) { - return optimize(gfg, KeyInfo(gfg), std::map()); + const KeyInfo &keyInfo, const std::map &lambda) { + return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /****************************************************************************/ diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 84ebe52cbe..758d2aec97 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -91,21 +91,15 @@ class IterativeSolver { virtual ~IterativeSolver() { } + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + boost::optional = boost::none, + boost::optional&> lambda = boost::none); + /* interface to the nonlinear optimizer, without initial estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda); - /* interface to the nonlinear optimizer, without damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo& keyInfo); - - /* interface to the nonlinear optimizer, without metadata and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - const std::map& lambda); - - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg); - /* interface to the nonlinear optimizer that the subclasses have to implement */ virtual VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 5312da34bc..448120cddd 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,17 +34,7 @@ string SlotEntry::toString() const { } /* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { - gttic(Scatter_Constructor); - - // If we have an ordering, pre-fill the ordered variables first - if (ordering) { - for (Key key : *ordering) { - add(key, 0); - } - } - +void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { // Now, find dimensions of variables and/or extend for (const auto& factor : gfg) { if (!factor) @@ -68,10 +58,30 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // To keep the same behavior as before, sort the keys after the ordering iterator first = begin(); - if (ordering) first += ordering->size(); + first += sortStart; if (first != end()) std::sort(first, end()); } +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg) { + gttic(Scatter_Constructor); + + ScatterHelper(gfg, 0); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg, + const Ordering& ordering) { + gttic(Scatter_Constructor); + + // pre-fill the ordered variables first + for (Key key : ordering) { + add(key, 0); + } + + ScatterHelper(gfg, ordering.size()); +} + /* ************************************************************************* */ void Scatter::add(Key key, size_t dim) { emplace_back(SlotEntry(key, dim)); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 793961c595..8936f827c0 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -23,8 +23,6 @@ #include #include -#include - namespace gtsam { class GaussianFactorGraph; @@ -53,15 +51,21 @@ class Scatter : public FastVector { /// Default Constructor Scatter() {} - /// Construct from gaussian factor graph, with optional (partial or complete) ordering - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); + /// Construct from gaussian factor graph, without ordering + explicit Scatter(const GaussianFactorGraph& gfg); + + /// Construct from gaussian factor graph, with (partial or complete) ordering + explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering); /// Add a key/dim pair void add(Key key, size_t dim); private: + /// Helper function for constructors, adds/finds dimensions of variables and + // sorts starting from sortStart + void ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart); + /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); }; diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index c24cb61433..c29a796237 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -26,37 +26,69 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) : values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); graph_ = *graph.linearize(solution); + computeBayesTree(); +} + +/* ************************************************************************* */ +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, + Factorization factorization) + : values_(solution), factorization_(factorization) { + gttic(MarginalsConstructor); + graph_ = *graph.linearize(solution); + computeBayesTree(ordering); +} + +/* ************************************************************************* */ +Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization) + : graph_(graph), values_(solution), factorization_(factorization) { + gttic(MarginalsConstructor); + computeBayesTree(); +} + +/* ************************************************************************* */ +Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, + Factorization factorization) + : graph_(graph), values_(solution), factorization_(factorization) { + gttic(MarginalsConstructor); computeBayesTree(ordering); } /* ************************************************************************* */ -Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) +Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization) : graph_(graph), factorization_(factorization) { gttic(MarginalsConstructor); - Values vals; for (const auto& keyValue: solution) { - vals.insert(keyValue.first, keyValue.second); + values_.insert(keyValue.first, keyValue.second); } - values_ = vals; - computeBayesTree(ordering); + computeBayesTree(); } /* ************************************************************************* */ -Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) - : graph_(graph), values_(solution), factorization_(factorization) { +Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, + Factorization factorization) + : graph_(graph), factorization_(factorization) { gttic(MarginalsConstructor); + for (const auto& keyValue: solution) { + values_.insert(keyValue.first, keyValue.second); + } computeBayesTree(ordering); } /* ************************************************************************* */ -void Marginals::computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering) { +void Marginals::computeBayesTree() { + // Compute BayesTree + if(factorization_ == CHOLESKY) + bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky); + else if(factorization_ == QR) + bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR); +} + +/* ************************************************************************* */ +void Marginals::computeBayesTree(const Ordering& ordering) { // Compute BayesTree if(factorization_ == CHOLESKY) bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); @@ -128,9 +160,9 @@ JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) co jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR); } else { if(factorization_ == CHOLESKY) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminatePreferCholesky)); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminatePreferCholesky)); else if(factorization_ == QR) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminateQR)); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminateQR)); } // Get information matrix diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 54a290196d..abad71ea7b 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -55,10 +55,33 @@ class GTSAM_EXPORT Marginals { * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). - * @param ordering An optional variable ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a nonlinear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering The ordering for elimination. + */ + Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering The ordering for elimination. + */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -66,8 +89,7 @@ class GTSAM_EXPORT Marginals { * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -75,8 +97,8 @@ class GTSAM_EXPORT Marginals { * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -103,7 +125,10 @@ class GTSAM_EXPORT Marginals { protected: /** Compute the Bayes Tree as a helper function to the constructor */ - void computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering); + void computeBayesTree(); + + /** Compute the Bayes Tree as a helper function to the constructor */ + void computeBayesTree(const Ordering& ordering); }; diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 0223250b54..525abd098e 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -14,7 +14,14 @@ using namespace std; namespace gtsam { /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment(OptionalOrdering ordering) const { + CSP::sharedValues CSP::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); + sharedValues mpe = chordal->optimize(); + return mpe; + } + + /// Find the best total assignment - can be expensive + CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); sharedValues mpe = chordal->optimize(); return mpe; diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index bbdadd3dc4..9e843f667f 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -60,7 +60,10 @@ namespace gtsam { // } /// Find the best total assignment - can be expensive - sharedValues optimalAssignment(OptionalOrdering ordering = boost::none) const; + sharedValues optimalAssignment() const; + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment(const Ordering& ordering) const; // /* // * Perform loopy belief propagation diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index c4e9d26f5a..bf968c8d7a 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,7 +206,7 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = fg.optimize(boost::none, EliminateCholesky); + VectorValues actual = fg.optimize(EliminateCholesky); // verify VectorValues expected = createCorrectDelta(); @@ -220,7 +220,7 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = fg.optimize(boost::none, EliminateQR); + VectorValues actual = fg.optimize(EliminateQR); // verify VectorValues expected = createCorrectDelta(); From 4877bdb4f3d7ec7daeb2f605997418fb93a52efb Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 20 Oct 2019 03:20:14 -0400 Subject: [PATCH 3/5] Caught some stragglers using boost::optional --- gtsam/discrete/DiscreteConditional.cpp | 19 ++++-- gtsam/discrete/DiscreteConditional.h | 6 +- gtsam/linear/Scatter.cpp | 7 +-- gtsam/linear/Scatter.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 53 +++++++++++----- gtsam/nonlinear/NonlinearFactorGraph.h | 22 +++++-- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- .../nonlinear/NonlinearClusterTree.h | 60 +++++++++++++++---- tests/testNonlinearFactorGraph.cpp | 2 +- 9 files changed, 131 insertions(+), 42 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index f12cd25675..71d04f246c 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -46,16 +46,25 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals, /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal, const boost::optional& orderedKeys) : + const DecisionTreeFactor& marginal) : BaseFactor( ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( joint.size()-marginal.size()) { if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout << (firstFrontalKey()) << endl; //TODO Print all keys - if (orderedKeys) { - keys_.clear(); - keys_.insert(keys_.end(), orderedKeys->begin(), orderedKeys->end()); - } +} + +/* ******************************************************************************** */ +DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const Ordering& orderedKeys) : + BaseFactor( + ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( + joint.size()-marginal.size()) { + if (ISDEBUG("DiscreteConditional::DiscreteConditional")) + cout << (firstFrontalKey()) << endl; //TODO Print all keys + + keys_.clear(); + keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end()); } /* ******************************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 88c3e56204..3da8d0a82f 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -60,7 +60,11 @@ class GTSAM_EXPORT DiscreteConditional: public DecisionTreeFactor, /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal, const boost::optional& orderedKeys = boost::none); + const DecisionTreeFactor& marginal); + + /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ + DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const Ordering& orderedKeys); /** * Combine several conditional into a single one. diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 448120cddd..d201dfa783 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,8 +34,7 @@ string SlotEntry::toString() const { } /* ************************************************************************* */ -void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { - // Now, find dimensions of variables and/or extend +void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { for (const auto& factor : gfg) { if (!factor) continue; @@ -66,7 +65,7 @@ void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { Scatter::Scatter(const GaussianFactorGraph& gfg) { gttic(Scatter_Constructor); - ScatterHelper(gfg, 0); + setDimensions(gfg, 0); } /* ************************************************************************* */ @@ -79,7 +78,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, add(key, 0); } - ScatterHelper(gfg, ordering.size()); + setDimensions(gfg, ordering.size()); } /* ************************************************************************* */ diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 8936f827c0..1583aa2f0d 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -64,7 +64,7 @@ class Scatter : public FastVector { /// Helper function for constructors, adds/finds dimensions of variables and // sorts starting from sortStart - void ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart); + void setDimensions(const GaussianFactorGraph& gfg, size_t sortStart); /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index a4bdd83e3f..3ad9cd9a61 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -344,23 +344,31 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li } /* ************************************************************************* */ -static Scatter scatterFromValues(const Values& values, boost::optional ordering) { +static Scatter scatterFromValues(const Values& values) { gttic(scatterFromValues); Scatter scatter; scatter.reserve(values.size()); - if (!ordering) { - // use "natural" ordering with keys taken from the initial values - for (const auto& key_value : values) { - scatter.add(key_value.key, key_value.value.dim()); - } - } else { - // copy ordering into keys and lookup dimension in values, is O(n*log n) - for (Key key : *ordering) { - const Value& value = values.at(key); - scatter.add(key, value.dim()); - } + // use "natural" ordering with keys taken from the initial values + for (const auto& key_value : values) { + scatter.add(key_value.key, key_value.value.dim()); + } + + return scatter; +} + +/* ************************************************************************* */ +static Scatter scatterFromValues(const Values& values, const Ordering& ordering) { + gttic(scatterFromValues); + + Scatter scatter; + scatter.reserve(values.size()); + + // copy ordering into keys and lookup dimension in values, is O(n*log n) + for (Key key : ordering) { + const Value& value = values.at(key); + scatter.add(key, value.dim()); } return scatter; @@ -368,7 +376,15 @@ static Scatter scatterFromValues(const Values& values, boost::optional ordering, const Dampen& dampen) const { + const Values& values, const Dampen& dampen) const { + KeyVector keys = values.keys(); + Ordering defaultOrdering(keys); + return linearizeToHessianFactor(values, defaultOrdering, dampen); +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Ordering& ordering, const Dampen& dampen) const { gttic(NonlinearFactorGraph_linearizeToHessianFactor); Scatter scatter = scatterFromValues(values, ordering); @@ -395,7 +411,16 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( /* ************************************************************************* */ Values NonlinearFactorGraph::updateCholesky(const Values& values, - boost::optional ordering, + const Dampen& dampen) const { + gttic(NonlinearFactorGraph_updateCholesky); + auto hessianFactor = linearizeToHessianFactor(values, dampen); + VectorValues delta = hessianFactor->solve(); + return values.retract(delta); +} + +/* ************************************************************************* */ +Values NonlinearFactorGraph::updateCholesky(const Values& values, + const Ordering& ordering, const Dampen& dampen) const { gttic(NonlinearFactorGraph_updateCholesky); auto hessianFactor = linearizeToHessianFactor(values, ordering, dampen); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 3234614590..0b0db1b7be 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -149,17 +149,31 @@ namespace gtsam { * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing * a new graph, and hence useful in case a dense solve is appropriate for your problem. - * An optional ordering can be given that still decides how the Hessian is laid out. * An optional lambda function can be used to apply damping on the filled Hessian. * No parallelism is exploited, because all the factors write in the same memory. */ boost::shared_ptr linearizeToHessianFactor( - const Values& values, boost::optional ordering = boost::none, - const Dampen& dampen = nullptr) const; + const Values& values, const Dampen& dampen = nullptr) const; + + /** + * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly + * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing + * a new graph, and hence useful in case a dense solve is appropriate for your problem. + * An ordering is given that still decides how the Hessian is laid out. + * An optional lambda function can be used to apply damping on the filled Hessian. + * No parallelism is exploited, because all the factors write in the same memory. + */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const; + + /// Linearize and solve in one pass. + /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values. + Values updateCholesky(const Values& values, + const Dampen& dampen = nullptr) const; /// Linearize and solve in one pass. /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values. - Values updateCholesky(const Values& values, boost::optional ordering = boost::none, + Values updateCholesky(const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const; /// Clone() performs a deep-copy of the graph, including all of the factors diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 180f4fb84d..9757cca73b 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -82,7 +82,7 @@ class NonlinearOptimizerParams { }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. inline bool isMultifrontal() const { diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h index a483c9521a..5d089f123d 100644 --- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h +++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h @@ -46,21 +46,26 @@ class NonlinearClusterTree : public ClusterTree { // linearize local custer factors straight into hessianFactor, which is returned // If no ordering given, uses colamd HessianFactor::shared_ptr linearizeToHessianFactor( - const Values& values, boost::optional ordering = boost::none, + const Values& values, const NonlinearFactorGraph::Dampen& dampen = nullptr) const { - if (!ordering) - ordering.reset(Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true)); - return factors.linearizeToHessianFactor(values, *ordering, dampen); + Ordering ordering; + ordering = Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true); + return factors.linearizeToHessianFactor(values, ordering, dampen); } - // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent - // TODO(frank): Use TBB to support deep trees and parallelism - std::pair linearizeAndEliminate( - const Values& values, boost::optional ordering = boost::none, + // linearize local custer factors straight into hessianFactor, which is returned + // If no ordering given, uses colamd + HessianFactor::shared_ptr linearizeToHessianFactor( + const Values& values, const Ordering& ordering, const NonlinearFactorGraph::Dampen& dampen = nullptr) const { - // Linearize and create HessianFactor f(front,separator) - HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen); + return factors.linearizeToHessianFactor(values, ordering, dampen); + } + // Helper function: recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, + const HessianFactor::shared_ptr& localFactor) const { // Get contributions f(front) from children, as well as p(children|front) GaussianBayesNet bayesNet; for (const auto& child : children) { @@ -72,12 +77,45 @@ class NonlinearClusterTree : public ClusterTree { return {bayesNet, localFactor}; } + // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + // Linearize and create HessianFactor f(front,separator) + HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, dampen); + return linearizeAndEliminate(values, localFactor); + } + + // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, const Ordering& ordering, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + // Linearize and create HessianFactor f(front,separator) + HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen); + return linearizeAndEliminate(values, localFactor); + } + + // Recursively eliminate subtree rooted at this Cluster + // Version that updates existing Bayes net and returns a new Hessian factor on parent clique + // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor + HessianFactor::shared_ptr linearizeAndEliminate( + const Values& values, GaussianBayesNet* bayesNet, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + auto bayesNet_newFactor_pair = linearizeAndEliminate(values, dampen); + if (bayesNet) { + bayesNet->push_back(bayesNet_newFactor_pair.first); + } + return bayesNet_newFactor_pair.second; + } + // Recursively eliminate subtree rooted at this Cluster // Version that updates existing Bayes net and returns a new Hessian factor on parent clique // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor HessianFactor::shared_ptr linearizeAndEliminate( const Values& values, GaussianBayesNet* bayesNet, - boost::optional ordering = boost::none, + const Ordering& ordering, const NonlinearFactorGraph::Dampen& dampen = nullptr) const { auto bayesNet_newFactor_pair = linearizeAndEliminate(values, ordering, dampen); if (bayesNet) { diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 290f0138e6..6a7a963bc0 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); } /* ************************************************************************* */ From 8e62a1405e5cc7b4a059b691ffda6e96d2b328f7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 28 Oct 2019 17:41:16 -0400 Subject: [PATCH 4/5] deprecated functions for backwards compatibility also removed some edits that were tangential to the PR. --- gtsam/base/TestableAssertions.h | 8 +- gtsam/discrete/DiscreteConditional.cpp | 7 +- .../inference/EliminateableFactorGraph-inst.h | 16 ++-- gtsam/inference/EliminateableFactorGraph.h | 73 ++++++++++++++++--- gtsam/linear/GaussianFactorGraph.cpp | 7 ++ gtsam/linear/GaussianFactorGraph.h | 6 ++ gtsam/linear/Scatter.cpp | 35 ++++----- gtsam/linear/Scatter.h | 5 -- gtsam/nonlinear/Marginals.h | 21 +++++- gtsam/nonlinear/NonlinearFactorGraph.cpp | 32 ++++---- gtsam/nonlinear/NonlinearFactorGraph.h | 20 +++++ gtsam/nonlinear/NonlinearOptimizer.cpp | 16 ++-- tests/testGaussianFactorGraphB.cpp | 2 + tests/testNonlinearFactorGraph.cpp | 2 +- 14 files changed, 172 insertions(+), 78 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index a698f8f984..1a31aa2848 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -71,8 +71,12 @@ bool assert_equal(const V& expected, const boost::optional& actual, double to } template -bool assert_equal(const V& expected, double tol = 1e-9) { - return false; +bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); } /** diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 71d04f246c..cf2997ce03 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -57,12 +57,7 @@ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, const DecisionTreeFactor& marginal, const Ordering& orderedKeys) : - BaseFactor( - ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( - joint.size()-marginal.size()) { - if (ISDEBUG("DiscreteConditional::DiscreteConditional")) - cout << (firstFrontalKey()) << endl; //TODO Print all keys - + DiscreteConditional(joint, marginal) { keys_.clear(); keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end()); } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index a77d96537c..81f4047a15 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,8 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - const Eliminate& function, OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const { + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because @@ -56,12 +56,12 @@ namespace gtsam { boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( const Ordering& ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + return eliminateSequential(ordering, function, computedVariableIndex); } else { gttic(eliminateSequential); // Do elimination @@ -81,8 +81,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - const Eliminate& function, OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check @@ -110,12 +110,12 @@ namespace gtsam { boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); + return eliminateMultifrontal(ordering, function, computedVariableIndex); } else { gttic(eliminateMultifrontal); // Do elimination with given ordering diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 8a216918c3..316ca8ee4d 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -89,7 +89,7 @@ namespace gtsam { typedef boost::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions - typedef const boost::optional& OptionalVariableIndex; + typedef boost::optional OptionalVariableIndex; /// Typedef for an optional ordering type typedef boost::optional OptionalOrderingType; @@ -115,9 +115,9 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateSequential( + OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of all variables to produce a Bayes net. * @@ -136,8 +136,7 @@ namespace gtsam { boost::shared_ptr eliminateSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; // orderingType is not necessary anymore, kept for backwards compatibility + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -156,9 +155,9 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateMultifrontal( + OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -172,8 +171,7 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; // orderingType no longer needed + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, @@ -241,7 +239,7 @@ namespace gtsam { * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, - const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -271,7 +269,7 @@ namespace gtsam { * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, - const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -288,6 +286,59 @@ namespace gtsam { // Access the derived factor graph class FactorGraphType& asDerived() { return static_cast(*this); } + + public: + /** \deprecated ordering and orderingType shouldn't both be specified */ + boost::shared_ptr eliminateSequential( + const Ordering& ordering, + const Eliminate& function, + OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + return eliminateSequential(ordering, function, variableIndex); + } + + /** \deprecated orderingType specified first for consistency */ + boost::shared_ptr eliminateSequential( + const Eliminate& function, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const { + return eliminateSequential(orderingType, function, variableIndex); + } + + /** \deprecated ordering and orderingType shouldn't both be specified */ + boost::shared_ptr eliminateMultifrontal( + const Ordering& ordering, + const Eliminate& function, + OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + return eliminateMultifrontal(ordering, function, variableIndex); + } + + /** \deprecated orderingType specified first for consistency */ + boost::shared_ptr eliminateMultifrontal( + const Eliminate& function, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const { + return eliminateMultifrontal(orderingType, function, variableIndex); + } + + /** \deprecated */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant variables, + boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const { + return marginalMultifrontalBayesNet(variables, function, variableIndex); + } + + /** \deprecated */ + boost::shared_ptr marginalMultifrontalBayesTree( + boost::variant variables, + boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const { + return marginalMultifrontalBayesTree(variables, function, variableIndex); + } }; } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index faa3f8bd60..8dc3600c68 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -494,4 +494,11 @@ namespace gtsam { return e; } + /* ************************************************************************* */ + /** \deprecated */ + VectorValues GaussianFactorGraph::optimize(boost::none_t, + const Eliminate& function) const { + return optimize(function); + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index f24fb602d0..2b9e8e6753 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -375,6 +375,12 @@ namespace gtsam { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } + public: + + /** \deprecated */ + VectorValues optimize(boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + }; /** diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index d201dfa783..07ecaf4838 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -33,8 +33,19 @@ string SlotEntry::toString() const { return oss.str(); } +Scatter::Scatter(const GaussianFactorGraph& gfg) : Scatter(gfg, Ordering()) {} + /* ************************************************************************* */ -void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { +Scatter::Scatter(const GaussianFactorGraph& gfg, + const Ordering& ordering) { + gttic(Scatter_Constructor); + + // If we have an ordering, pre-fill the ordered variables first + for (Key key : ordering) { + add(key, 0); + } + + // Now, find dimensions of variables and/or extend for (const auto& factor : gfg) { if (!factor) continue; @@ -57,30 +68,10 @@ void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { // To keep the same behavior as before, sort the keys after the ordering iterator first = begin(); - first += sortStart; + first += ordering.size(); if (first != end()) std::sort(first, end()); } -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg) { - gttic(Scatter_Constructor); - - setDimensions(gfg, 0); -} - -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - const Ordering& ordering) { - gttic(Scatter_Constructor); - - // pre-fill the ordered variables first - for (Key key : ordering) { - add(key, 0); - } - - setDimensions(gfg, ordering.size()); -} - /* ************************************************************************* */ void Scatter::add(Key key, size_t dim) { emplace_back(SlotEntry(key, dim)); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 1583aa2f0d..3b34d170c2 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -61,11 +61,6 @@ class Scatter : public FastVector { void add(Key key, size_t dim); private: - - /// Helper function for constructors, adds/finds dimensions of variables and - // sorts starting from sortStart - void setDimensions(const GaussianFactorGraph& gfg, size_t sortStart); - /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); }; diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index abad71ea7b..4e201cc389 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -64,7 +64,7 @@ class GTSAM_EXPORT Marginals { * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering The ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. @@ -80,7 +80,7 @@ class GTSAM_EXPORT Marginals { * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering The ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. @@ -97,7 +97,7 @@ class GTSAM_EXPORT Marginals { * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** print */ @@ -121,7 +121,7 @@ class GTSAM_EXPORT Marginals { /** Optimize the bayes tree */ VectorValues optimize() const; - + protected: /** Compute the Bayes Tree as a helper function to the constructor */ @@ -130,6 +130,19 @@ class GTSAM_EXPORT Marginals { /** Compute the Bayes Tree as a helper function to the constructor */ void computeBayesTree(const Ordering& ordering); +public: + /** \deprecated argument order changed due to removing boost::optional */ + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, + const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} + + /** \deprecated argument order changed due to removing boost::optional */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, + const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} + + /** \deprecated argument order changed due to removing boost::optional */ + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, + const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} + }; /** diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 3ad9cd9a61..d4b9fbb68e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -376,19 +376,7 @@ static Scatter scatterFromValues(const Values& values, const Ordering& ordering) /* ************************************************************************* */ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( - const Values& values, const Dampen& dampen) const { - KeyVector keys = values.keys(); - Ordering defaultOrdering(keys); - return linearizeToHessianFactor(values, defaultOrdering, dampen); -} - -/* ************************************************************************* */ -HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( - const Values& values, const Ordering& ordering, const Dampen& dampen) const { - gttic(NonlinearFactorGraph_linearizeToHessianFactor); - - Scatter scatter = scatterFromValues(values, ordering); - + const Values& values, const Scatter& scatter, const Dampen& dampen) const { // NOTE(frank): we are heavily leaning on friendship below HessianFactor::shared_ptr hessianFactor(new HessianFactor(scatter)); @@ -409,6 +397,24 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( return hessianFactor; } +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Ordering& order, const Dampen& dampen) const { + gttic(NonlinearFactorGraph_linearizeToHessianFactor); + + Scatter scatter = scatterFromValues(values, order); + return linearizeToHessianFactor(values, scatter, dampen); +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Dampen& dampen) const { + gttic(NonlinearFactorGraph_linearizeToHessianFactor); + + Scatter scatter = scatterFromValues(values); + return linearizeToHessianFactor(values, scatter, dampen); +} + /* ************************************************************************* */ Values NonlinearFactorGraph::updateCholesky(const Values& values, const Dampen& dampen) const { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 0b0db1b7be..902a47a0f3 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -204,6 +204,13 @@ namespace gtsam { private: + /** + * Linearize from Scatter rather than from Ordering. Made private because + * it doesn't include gttic. + */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const; + /** Serialization function */ friend class boost::serialization::access; template @@ -211,6 +218,19 @@ namespace gtsam { ar & boost::serialization::make_nvp("NonlinearFactorGraph", boost::serialization::base_object(*this)); } + + public: + + /** \deprecated */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, boost::none_t, const Dampen& dampen = nullptr) const + {return linearizeToHessianFactor(values, dampen);} + + /** \deprecated */ + Values updateCholesky(const Values& values, boost::none_t, + const Dampen& dampen = nullptr) const + {return updateCholesky(values, dampen);} + }; /// traits diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b12dcf70a3..ad3b9c4cc0 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -128,18 +128,22 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point VectorValues delta; - Ordering ordering; - if (params.ordering) - ordering = *params.ordering; // Check which solver we are using if (params.isMultifrontal()) { // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.optimize(ordering, params.getEliminationFunction()); + if (params.ordering) + delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + else + delta = gfg.optimize(params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(ordering, params.getEliminationFunction(), boost::none, - params.orderingType)->optimize(); + if (params.ordering) + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), + boost::none, params.orderingType)->optimize(); + else + delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, + params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index bf968c8d7a..9596f4af56 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,6 +206,7 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph + VectorValues actual1 = fg.optimize(boost::none, EliminateCholesky); VectorValues actual = fg.optimize(EliminateCholesky); // verify @@ -220,6 +221,7 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph + VectorValues actual1 = fg.optimize(boost::none, EliminateQR); VectorValues actual = fg.optimize(EliminateQR); // verify diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 6a7a963bc0..290f0138e6 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); } /* ************************************************************************* */ From e40a5008590e427c2d56b5a5c91a432b0cf6342a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 28 Oct 2019 17:43:56 -0400 Subject: [PATCH 5/5] Update test cases for new preferred syntax --- tests/testGaussianFactorGraphB.cpp | 2 -- tests/testNonlinearFactorGraph.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 9596f4af56..bf968c8d7a 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,7 +206,6 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual1 = fg.optimize(boost::none, EliminateCholesky); VectorValues actual = fg.optimize(EliminateCholesky); // verify @@ -221,7 +220,6 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual1 = fg.optimize(boost::none, EliminateQR); VectorValues actual = fg.optimize(EliminateQR); // verify diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 290f0138e6..6a7a963bc0 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); } /* ************************************************************************* */