diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index eb2285b28..906d620cc 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -78,7 +78,7 @@ struct GTSAM_EXPORT DeltaImpl { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional > constrainedKeys; + std::optional > constrainedKeys; }; /** @@ -195,9 +195,9 @@ struct GTSAM_EXPORT UpdateImpl { // Calculate nonlinear error void error(const NonlinearFactorGraph& nonlinearFactors, - const Values& estimate, boost::optional* result) const { + const Values& estimate, std::optional* result) const { gttic(error); - result->reset(nonlinearFactors.error(estimate)); + *result = nonlinearFactors.error(estimate); } // Mark linear update diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d5e25cef8..de32aea9c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -396,9 +396,9 @@ void ISAM2::removeVariables(const KeySet& unusedKeys) { ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, - const boost::optional >& constrainedKeys, - const boost::optional >& noRelinKeys, - const boost::optional >& extraReelimKeys, + const std::optional >& constrainedKeys, + const std::optional >& noRelinKeys, + const std::optional >& extraReelimKeys, bool force_relinearize) { ISAM2UpdateParams params; params.constrainedKeys = constrainedKeys; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index dd246617a..8b48f2f7d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -87,7 +87,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { ISAM2Params params_; /** The current Dogleg Delta (trust region radius) */ - mutable boost::optional doglegDelta_; + mutable std::optional doglegDelta_; /** Set of variables that are involved with linear factors from marginalized * variables and thus cannot have their linearization points changed. */ @@ -152,9 +152,9 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FactorIndices& removeFactorIndices = FactorIndices(), - const boost::optional >& constrainedKeys = boost::none, - const boost::optional >& noRelinKeys = boost::none, - const boost::optional >& extraReelimKeys = boost::none, + const std::optional >& constrainedKeys = {}, + const std::optional >& noRelinKeys = {}, + const std::optional >& extraReelimKeys = {}, bool force_relinearize = false); /** diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 077174970..26049f9da 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -51,7 +51,7 @@ struct ISAM2Result { * ISAM2Params::evaluateNonlinearError is set to \c true, because there is * some cost to this computation. */ - boost::optional errorBefore; + std::optional errorBefore; /** The nonlinear error of all of the factors computed after the current * update, meaning that variables above the relinearization threshold @@ -63,7 +63,7 @@ struct ISAM2Result { * ISAM2Params::evaluateNonlinearError is set to \c true, because there is * some cost to this computation. */ - boost::optional errorAfter; + std::optional errorAfter; /** The number of variables that were relinearized because their linear * deltas exceeded the reslinearization threshold @@ -155,14 +155,20 @@ struct ISAM2Result { /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See * Detail for information about the results data stored here. */ - boost::optional detail; + std::optional detail; explicit ISAM2Result(bool enableDetailedResults = false) { - if (enableDetailedResults) detail.reset(DetailedResults()); + if (enableDetailedResults) detail = DetailedResults(); } /// Return pointer to detail, 0 if no detail requested - DetailedResults* details() { return detail.get_ptr(); } + DetailedResults* details() { + if (detail.has_value()) { + return &(*detail); + } else { + return nullptr; + } + } /// Print results void print(const std::string str = "") const { diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 0a3be1daf..575c99be5 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -37,16 +37,16 @@ struct ISAM2UpdateParams { /** An optional map of keys to group labels, such that a variable can be * constrained to a particular grouping in the BayesTree */ - boost::optional> constrainedKeys{boost::none}; + std::optional> constrainedKeys{{}}; /** An optional set of nonlinear keys that iSAM2 will hold at a constant * linearization point, regardless of the size of the linear delta */ - boost::optional> noRelinKeys{boost::none}; + std::optional> noRelinKeys{{}}; /** An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless * of the size of the linear delta. This allows the provided keys to be * reordered. */ - boost::optional> extraReelimKeys{boost::none}; + std::optional> extraReelimKeys{{}}; /** Relinearize any variables whose delta magnitude is sufficiently large * (Params::relinearizeThreshold), regardless of the relinearization @@ -63,7 +63,7 @@ struct ISAM2UpdateParams { * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ - boost::optional> newAffectedKeys{boost::none}; + std::optional> newAffectedKeys{{}}; /** By default, iSAM2 uses a wildfire update scheme that stops updating when * the deltas become too small down in the tree. This flagg forces a full diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 218230421..6809137ef 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -104,7 +104,7 @@ public: }; LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) + std::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. NonlinearOptimizerParams() = default; diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 2943b5e68..17557ba3a 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -26,6 +26,7 @@ #include #include +#include namespace gtsam { namespace internal { @@ -132,12 +133,12 @@ class ExecutionTrace { /// Return record pointer, quite unsafe, used only for testing template - boost::optional record() { + std::optional record() { if (kind != Function) - return boost::none; + return {}; else { Record* p = dynamic_cast(content.ptr); - return p ? boost::optional(p) : boost::none; + return p ? std::optional(p) : std::nullopt; } } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index b9cf66a97..6ef4f9066 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -43,7 +43,7 @@ bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol /* ************************************************************************* */ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) { + const std::optional >& keysToMove, const std::optional< FactorIndices >& removeFactorIndices) { gttic(update); @@ -63,7 +63,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No } // Generate ordering constraints that force the 'keys to move' to the end - boost::optional > orderingConstraints = boost::none; + std::optional > orderingConstraints = {}; if(keysToMove && keysToMove->size() > 0) { orderingConstraints = FastMap(); int group = 1; @@ -86,10 +86,10 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No // Create the set of linear keys that iSAM2 should hold constant // iSAM2 takes care of this for us; no need to specify additional noRelin keys - boost::optional > noRelinKeys = boost::none; + std::optional > noRelinKeys = {}; // Mark additional keys between the 'keys to move' and the leaves - boost::optional > additionalKeys = boost::none; + std::optional > additionalKeys = {}; if(keysToMove && keysToMove->size() > 0) { std::set markedKeys; for(Key key: *keysToMove) { @@ -211,7 +211,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth // Remove the old factors on the separator and insert the new ones FactorIndices removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); - ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false); + ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, {}, noRelinKeys, {}, false); currentSmootherSummarizationSlots_ = result.newFactorsIndices; // Set the previous smoother summarization to the current smoother summarization and clear the smoother shortcut diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index eb1174749..8e1409523 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -123,8 +123,8 @@ public: * @param removeFactorIndices An optional set of indices corresponding to the factors you want to remove from the graph */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional >& keysToMove = boost::none, - const boost::optional< FactorIndices >& removeFactorIndices = boost::none); + const std::optional >& keysToMove = {}, + const std::optional< FactorIndices >& removeFactorIndices = {}); /** * Perform any required operations before the synchronization process starts. diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index d29dfe8ca..52e56260d 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -76,7 +76,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( } FastVector removedFactors; - boost::optional > constrainedKeys = boost::none; + std::optional > constrainedKeys = {}; // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); @@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Update iSAM2 isamResult_ = isam_.update(newFactors, newTheta, - factorsToRemove, constrainedKeys, boost::none, additionalMarkedKeys); + factorsToRemove, constrainedKeys, {}, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, @@ -175,7 +175,7 @@ void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { /* ************************************************************************* */ void IncrementalFixedLagSmoother::createOrderingConstraints( const KeyVector& marginalizableKeys, - boost::optional >& constrainedKeys) const { + std::optional >& constrainedKeys) const { if (marginalizableKeys.size() > 0) { constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 4079dbb23..b2910b139 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -137,7 +137,7 @@ protected: /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ void createOrderingConstraints(const KeyVector& marginalizableKeys, - boost::optional >& constrainedKeys) const; + std::optional >& constrainedKeys) const; private: /** Private methods for printing debug information */ diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index bb35ada19..5a3678392 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -204,7 +204,7 @@ TEST(ExpressionFactor, Binary) { expected22 << 1, 0, 0, 1; // Check matrices - boost::optional r = trace.record(); + std::optional r = trace.record(); CHECK(r); EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); @@ -257,7 +257,7 @@ TEST(ExpressionFactor, Shallow) { expected23 << 1, 0, 0, 0, 1, 0; // Check matrices - boost::optional r = trace.record(); + std::optional r = trace.record(); CHECK(r); EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9)); @@ -623,7 +623,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { CHECK(assert_equal(A * b, Ab)); CHECK(assert_equal( numericalDerivative11( - std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a), + [&](const Point2& a) { return f(a, b, {}, {}); }, a), H1)); Values values;