fixed smart factor and camera set for boost optional references

gtsam/geometry/CameraSet.h

@@ -106,8 +106,8 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
    */
   template <class POINT>
   ZVector project2(const POINT& point,                          //
-                   boost::optional<FBlocks&> Fs = boost::none,  //
+                   FBlocks* Fs = nullptr,  //
-                   boost::optional<Matrix&> E = boost::none) const {
+                   Matrix* E = nullptr) const {
     static const int N = FixedDimension<POINT>::value;
 
     // Allocate result
@@ -131,14 +131,35 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
     return z;
   }
 
+  /** An overload o the project2 function to accept
+   * full matrices and vectors and pass it to the pointer
+   * version of the function
+   */
+  template <class POINT, class... OptArgs>
+  ZVector project2(const POINT& point, OptArgs&... args) const {
+    // pass it to the pointer version of the function
+    return project2(point, (&args)...);
+  }
+
   /// Calculate vector [project2(point)-z] of re-projection errors
   template <class POINT>
   Vector reprojectionError(const POINT& point, const ZVector& measured,
-                           boost::optional<FBlocks&> Fs = boost::none,  //
+                           FBlocks* Fs = nullptr,  //
-                           boost::optional<Matrix&> E = boost::none) const {
+                           Matrix* E = nullptr) const {
     return ErrorVector(project2(point, Fs, E), measured);
   }
 
+  /** An overload o the reprojectionError function to accept
+   * full matrices and vectors and pass it to the pointer
+   * version of the function
+   */
+  template <class POINT, class... OptArgs>
+  Vector reprojectionError(const POINT& point, const ZVector& measured,
+                           OptArgs&... args) const {
+    // pass it to the pointer version of the function
+    return reprojectionError(point, measured, (&args)...);
+  }
+
   /**
    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
    * G = F' * F - F' * E * P * E' * F

gtsam/nonlinear/Expression-inl.h

@@ -138,8 +138,7 @@ void Expression<T>::print(const std::string& s) const {
 
 template<typename T>
 T Expression<T>::value(const Values& values,
-    boost::optional<std::vector<Matrix>&> H) const {
+    std::vector<Matrix>* H) const {
-
   if (H) {
     // Call private version that returns derivatives in H
     KeyVector keys;

gtsam/nonlinear/Expression.h

@@ -155,8 +155,15 @@ public:
    * Notes: this is not terribly efficient, and H should have correct size.
    * The order of the Jacobians is same as keys in either keys() or dims()
    */
-  T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
+  T value(const Values& values, std::vector<Matrix>* H = nullptr) const;
-      boost::none) const;
+
+  /**
+   * An overload of the value function to accept reference to vector of matrices instead of
+   * a pointer to vector of matrices.
+   */
+  T value(const Values& values, std::vector<Matrix>& H) const {
+    return value(values, &H);
+  }
 
   /**
    *  @return a "deep" copy of this Expression

gtsam/nonlinear/ISAM2.cpp

@@ -478,8 +478,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
 /* ************************************************************************* */
 void ISAM2::marginalizeLeaves(
     const FastList<Key>& leafKeysList,
-    boost::optional<FactorIndices&> marginalFactorsIndices,
+    FactorIndices* marginalFactorsIndices,
-    boost::optional<FactorIndices&> deletedFactorsIndices) {
+    FactorIndices* deletedFactorsIndices) {
   // Convert to ordered set
   KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());
 

gtsam/nonlinear/ISAM2.h

@@ -198,8 +198,20 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
    */
   void marginalizeLeaves(
       const FastList<Key>& leafKeys,
-      boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
+      FactorIndices* marginalFactorsIndices = nullptr,
-      boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
+      FactorIndices* deletedFactorsIndices = nullptr);
+
+  /** An overload of marginalizeLeaves that takes references
+   * to vectors instead of pointers to vectors and passes
+   * it to the pointer version of the function.
+   */
+  template <class... OptArgs>
+      void marginalizeLeaves(const FastList<Key>& leafKeys,
+                             OptArgs&&... optArgs) {
+          // dereference the optional arguments and pass
+          // it to the pointer version
+          marginalizeLeaves(leafKeys, (&optArgs)...);
+      }
 
   /// Access the current linearization point
   const Values& getLinearizationPoint() const { return theta_; }

gtsam/nonlinear/Values-inl.h

@@ -373,7 +373,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<typename ValueType>
-  boost::optional<const ValueType&> Values::exists(Key j) const {
+  const ValueType * Values::exists(Key j) const {
     // Find the item
     KeyValueMap::const_iterator item = values_.find(j);
 
@@ -381,14 +381,14 @@ namespace gtsam {
       // dynamic cast the type and throw exception if incorrect
       auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
       if (ptr) {
-        return ptr->value();
+        return &ptr->value();
       } else {
         // NOTE(abe): clang warns about potential side effects if done in typeid
         const Value* value = item->second;
         throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
       }
      } else {
-      return boost::none;
+      return nullptr;
     }
   }
 

gtsam/nonlinear/Values.h

@@ -184,12 +184,12 @@ namespace gtsam {
      * exists. */
     bool exists(Key j) const;
 
-    /** Check if a value with key \c j exists, returns the value with type
+    /** Check if a value with key \c j exists, returns a pointer to the const version of the value
-     * \c Value if the key does exist, or boost::none if it does not exist.
+     * \c Value if the key does exist, or nullptr if it does not exist.
      * Throws DynamicValuesIncorrectType if the value type associated with the
      * requested key does not match the stored value type. */
     template<typename ValueType>
-    boost::optional<const ValueType&> exists(Key j) const;
+    const ValueType * exists(Key j) const;
 
     /** Find an element by key, returning an iterator, or end() if the key was
      * not found. */

gtsam/nonlinear/tests/testValues.cpp

@@ -308,7 +308,7 @@ TEST(Values, exists_)
   config0.insert(key1, 1.0);
   config0.insert(key2, 2.0);
 
-  boost::optional<const double&> v = config0.exists<double>(key1);
+  const double* v = config0.exists<double>(key1);
   DOUBLES_EQUAL(1.0,*v,1e-9);
 }
 

gtsam/slam/SmartFactorBase.h

@@ -198,13 +198,16 @@ protected:
     }
   }
 
-  /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
+  /** Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
-  /// derivatives. This is the error before the noise model is applied.
+  * derivatives. This is the error before the noise model is applied.
+  * The templated version described above must finally get resolved to this
+  * function.
+  */
   template <class POINT>
   Vector unwhitenedError(
       const Cameras& cameras, const POINT& point,
-      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,  //
+      typename Cameras::FBlocks* Fs = nullptr,  //
-      boost::optional<Matrix&> E = boost::none) const {
+      Matrix* E = nullptr) const {
     // Reproject, with optional derivatives.
     Vector error = cameras.reprojectionError(point, measured_, Fs, E);
 
@@ -233,6 +236,19 @@ protected:
     return error;
   }
 
+  /** 
+   * An overload of unwhitenedError. This allows
+   * end users to provide optional arguments that are l-value references
+   * to the matrices and vectors that will be used to store the results instead
+   * of pointers.
+   */
+  template<class POINT, class ...OptArgs>
+  Vector unwhitenedError(
+      const Cameras& cameras, const POINT& point,
+      OptArgs&&... optArgs) const {
+    return unwhitenedError(cameras, point, (&optArgs)...);
+  }
+
   /**
    * This corrects the Jacobians for the case in which some 2D measurement is
    * missing (nan). In practice, this does not do anything in the monocular
@@ -240,8 +256,21 @@ protected:
    */
   virtual void correctForMissingMeasurements(
       const Cameras& cameras, Vector& ue,
-      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+      typename Cameras::FBlocks* Fs = nullptr,
-      boost::optional<Matrix&> E = boost::none) const {}
+      Matrix* E = nullptr) const {}
+
+  /**
+   * An overload of correctForMissingMeasurements. This allows
+   * end users to provide optional arguments that are l-value references
+   * to the matrices and vectors that will be used to store the results instead
+   * of pointers.
+   */
+  template<class ...OptArgs>
+  void correctForMissingMeasurements(
+      const Cameras& cameras, Vector& ue,
+      OptArgs&&... optArgs) const {
+    correctForMissingMeasurements(cameras, ue, (&optArgs)...);
+  }
 
   /**
    * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) -
@@ -288,7 +317,7 @@ protected:
     // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
     // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
     //                                         = |A*dx - (z-h(x_bar))|
-    b = -unwhitenedError(cameras, point, Fs, E);
+    b = -unwhitenedError(cameras, point, &Fs, &E);
   }
 
   /**

gtsam/slam/SmartProjectionFactor.h

@@ -337,8 +337,9 @@ protected:
    */
   bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
     bool nonDegenerate = triangulateForLinearize(cameras);
-    if (nonDegenerate)
+    if (nonDegenerate) {
-      cameras.project2(*result_, boost::none, E);
+      cameras.project2(*result_, nullptr, &E);
+    }
     return nonDegenerate;
   }
 

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -340,7 +340,7 @@ public:
   bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
     bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)
-      cameras.project2(*result_, boost::none, E);
+      cameras.project2(*result_, nullptr, &E);
     return nonDegenerate;
   }
 
@@ -453,8 +453,8 @@ public:
    */
   void correctForMissingMeasurements(
       const Cameras& cameras, Vector& ue,
-      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+      typename Cameras::FBlocks* Fs = nullptr,
-      boost::optional<Matrix&> E = boost::none) const override {
+      Matrix* E = nullptr) const override {
     // when using stereo cameras, some of the measurements might be missing:
     for (size_t i = 0; i < cameras.size(); i++) {
       const StereoPoint2& z = measured_.at(i);