nonlinear

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<FastMap<Key, int> > constrainedKeys;
+    std::optional<FastMap<Key, int> > constrainedKeys;
   };
 
   /**
@@ -195,9 +195,9 @@ struct GTSAM_EXPORT UpdateImpl {
 
   // Calculate nonlinear error
   void error(const NonlinearFactorGraph& nonlinearFactors,
-             const Values& estimate, boost::optional<double>* result) const {
+             const Values& estimate, std::optional<double>* result) const {
     gttic(error);
-    result->reset(nonlinearFactors.error(estimate));
+    *result = nonlinearFactors.error(estimate);
   }
 
   // Mark linear update

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<FastMap<Key, int> >& constrainedKeys,
-    const boost::optional<FastList<Key> >& noRelinKeys,
-    const boost::optional<FastList<Key> >& extraReelimKeys,
+    const std::optional<FastMap<Key, int> >& constrainedKeys,
+    const std::optional<FastList<Key> >& noRelinKeys,
+    const std::optional<FastList<Key> >& extraReelimKeys,
     bool force_relinearize) {
   ISAM2UpdateParams params;
   params.constrainedKeys = constrainedKeys;

gtsam/nonlinear/ISAM2.h

@@ -87,7 +87,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   ISAM2Params params_;
 
   /** The current Dogleg Delta (trust region radius) */
-  mutable boost::optional<double> doglegDelta_;
+  mutable std::optional<double> 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<ISAM2Clique> {
       const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
       const Values& newTheta = Values(),
       const FactorIndices& removeFactorIndices = FactorIndices(),
-      const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
-      const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
-      const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
+      const std::optional<FastMap<Key, int> >& constrainedKeys = {},
+      const std::optional<FastList<Key> >& noRelinKeys = {},
+      const std::optional<FastList<Key> >& extraReelimKeys = {},
       bool force_relinearize = false);
 
   /**

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<double> errorBefore;
+  std::optional<double> 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<double> errorAfter;
+  std::optional<double> 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<DetailedResults> detail;
+  std::optional<DetailedResults> 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 {

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<FastMap<Key, int>> constrainedKeys{boost::none};
+  std::optional<FastMap<Key, int>> 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<FastList<Key>> noRelinKeys{boost::none};
+  std::optional<FastList<Key>> 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<FastList<Key>> extraReelimKeys{boost::none};
+  std::optional<FastList<Key>> 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<FastMap<FactorIndex, KeySet>> newAffectedKeys{boost::none};
+  std::optional<FastMap<FactorIndex, KeySet>> 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

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> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty)
+  std::optional<Ordering> 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;

gtsam/nonlinear/internal/ExecutionTrace.h

@@ -26,6 +26,7 @@
 
 #include <Eigen/Core>
 #include <iostream>
+#include <optional>
 
 namespace gtsam {
 namespace internal {
@@ -132,12 +133,12 @@ class ExecutionTrace {
 
   /// Return record pointer, quite unsafe, used only for testing
   template<class Record>
-  boost::optional<Record*> record() {
+  std::optional<Record*> record() {
     if (kind != Function)
-      return boost::none;
+      return {};
     else {
       Record* p = dynamic_cast<Record*>(content.ptr);
-      return p ? boost::optional<Record*>(p) : boost::none;
+      return p ? std::optional<Record*>(p) : std::nullopt;
     }
   }
 

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<FastList<Key> >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) {
+    const std::optional<FastList<Key> >& 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<FastMap<Key,int> > orderingConstraints = boost::none;
+  std::optional<FastMap<Key,int> > orderingConstraints = {};
   if(keysToMove && keysToMove->size() > 0) {
     orderingConstraints = FastMap<Key,int>();
     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<FastList<Key> > noRelinKeys = boost::none;
+  std::optional<FastList<Key> > noRelinKeys = {};
 
   // Mark additional keys between the 'keys to move' and the leaves
-  boost::optional<FastList<Key> > additionalKeys = boost::none;
+  std::optional<FastList<Key> > additionalKeys = {};
   if(keysToMove && keysToMove->size() > 0) {
     std::set<Key> 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

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<FastList<Key> >& keysToMove = boost::none,
-      const boost::optional< FactorIndices >& removeFactorIndices = boost::none);
+      const std::optional<FastList<Key> >& keysToMove = {},
+      const std::optional< FactorIndices >& removeFactorIndices = {});
 
   /**
    * Perform any required operations before the synchronization process starts.

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp

@@ -76,7 +76,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
   }
 
   FastVector<size_t> removedFactors;
-  boost::optional<FastMap<Key, int> > constrainedKeys = boost::none;
+  std::optional<FastMap<Key, int> > 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<FastMap<Key, int> >& constrainedKeys) const {
+    std::optional<FastMap<Key, int> >& constrainedKeys) const {
   if (marginalizableKeys.size() > 0) {
     constrainedKeys = FastMap<Key, int>();
     // Generate ordering constraints so that the marginalizable variables will be eliminated first

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<FastMap<Key, int> >& constrainedKeys) const;
+      std::optional<FastMap<Key, int> >& constrainedKeys) const;
 
 private:
   /** Private methods for printing debug information */

tests/testExpressionFactor.cpp

@@ -204,7 +204,7 @@ TEST(ExpressionFactor, Binary) {
   expected22 << 1, 0, 0, 1;
 
   // Check matrices
-  boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
+  std::optional<Binary::Record*> r = trace.record<Binary::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<Unary::Record*> r = trace.record<Unary::Record>();
+  std::optional<Unary::Record*> r = trace.record<Unary::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<Vector3, Point2>(
-          std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a),
+        [&](const Point2& a) { return f(a, b, {}, {}); }, a),
       H1));
 
   Values values;