From 5a358489dc91daeaafd6b6dcb6f84670d479e104 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 13 Oct 2019 23:54:22 -0400 Subject: [PATCH] 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 1a31aa284..a698f8f98 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 8084aa75b..249c594b6 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 e9938ceb6..04094d593 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 669c8a964..9da7a1609 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 9281c7e7a..fe0a9b2df 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, optionalOrdering); + 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); 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 49cba482d..8bdf4e475 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,16 @@ 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 @@ -223,7 +241,20 @@ 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 Ordering& ordering) 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; /** * Return the dense Hessian \f$ \Lambda \f$ and information vector @@ -231,7 +262,15 @@ 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 Ordering& ordering) 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; /** 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 c208259b8..cc82e2fa0 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 cb9da0f1a..f0c79d8fb 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 c7d4e5405..9478f6fbf 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 758d2aec9..84ebe52cb 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -91,15 +91,21 @@ public: 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 06f6b3bed..9be010ff5 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 36d1e12da..c8d77c554 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 1fe7009bc..a24acfacd 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -77,11 +77,17 @@ public: /// 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 e1efa2061..b12dcf70a 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