Merge pull request #1392 from kartikarcot/verdant/typedef-optional-re…

…ferences

CMakeLists.txt

@@ -6,6 +6,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
   set(CMAKE_MACOSX_RPATH 0)
 endif()
 
+set(CMAKE_CXX_STANDARD 17)
+
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 3)
@@ -87,7 +89,7 @@ add_subdirectory(timing)
 
 # Build gtsam_unstable
 if (GTSAM_BUILD_UNSTABLE)
-    add_subdirectory(gtsam_unstable)
+  add_subdirectory(gtsam_unstable)
 endif()
 
 # This is the new wrapper

doc/Code/LocalizationFactor.cpp

@@ -2,11 +2,14 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   double mx_, my_; ///< X and Y measurements
 
 public:
+
+  // Provide access to the Matrix& version of evaluateError:
+  using gtsam::NoiseModelFactor1<Pose2>::evaluateError;
+
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
-  Vector evaluateError(const Pose2& q, 
-      boost::optional<Matrix&> H = boost::none) const override {
+  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
     const Rot2& R = q.rotation();
     if (H) (*H) = (gtsam::Matrix(2, 3) <<
             R.c(), -R.s(), 0.0,

examples/CameraResectioning.cpp

@@ -46,10 +46,9 @@ class ResectioningFactor: public NoiseModelFactorN<Pose3> {
   }
 
   /// evaluate the error
-  Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
-      boost::none) const override {
+  Vector evaluateError(const Pose3& pose, OptionalMatrixType H) const override {
     PinholeCamera<Cal3_S2> camera(pose, *K_);
-    return camera.project(P_, H, boost::none, boost::none) - p_;
+    return camera.project(P_, H, OptionalNone, OptionalNone) - p_;
   }
 };
 

examples/LocalizationExample.cpp

@@ -71,6 +71,10 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
   double mx_, my_;
 
  public:
+
+  // Provide access to Matrix& version of evaluateError:
+  using NoiseModelFactor1<Pose2>::evaluateError;
+
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<UnaryFactor> shared_ptr;
 
@@ -84,7 +88,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
-  Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const override {
+  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
     // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
     // The error is then simply calculated as E(q) = h(q) - m:
     // error_x = q.x - mx

gtsam/base/OptionalJacobian.h

@@ -94,8 +94,10 @@ class OptionalJacobian {
   /// Constructor that will resize a dynamic matrix (unless already correct)
   OptionalJacobian(Eigen::MatrixXd* dynamic) :
       map_(nullptr) {
-    dynamic->resize(Rows, Cols); // no malloc if correct size
-    usurp(dynamic->data());
+    if (dynamic) {
+      dynamic->resize(Rows, Cols);  // no malloc if correct size
+      usurp(dynamic->data());
+    }
   }
 
   /**

gtsam/base/numericalDerivative.h

@@ -35,7 +35,7 @@ namespace gtsam {
  *
  * Usage of the boost bind version to rearrange arguments:
  *   for a function with one relevant param and an optional derivative:
- *     Foo bar(const Obj& a, boost::optional<Matrix&> H1)
+ *     Foo bar(const Obj& a, OptionalMatrixType H1)
  *   Use boost.bind to restructure:
  *     std::bind(bar, std::placeholders::_1, boost::none)
  *   This syntax will fix the optional argument to boost::none, while using the first argument provided

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,  //
-                   boost::optional<Matrix&> E = boost::none) const {
+                   FBlocks* Fs = nullptr,  //
+                   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,  //
-                           boost::optional<Matrix&> E = boost::none) const {
+                           FBlocks* Fs = nullptr,  //
+                           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/inference/VariableIndex-inl.h

@@ -24,8 +24,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class FG>
-void VariableIndex::augment(const FG& factors,
-    boost::optional<const FactorIndices&> newFactorIndices) {
+void VariableIndex::augment(const FG& factors, const FactorIndices* newFactorIndices) {
   gttic(VariableIndex_augment);
 
   // Augment index for each factor

gtsam/inference/VariableIndex.h

@@ -126,7 +126,16 @@ class GTSAM_EXPORT VariableIndex {
    * solving problems incrementally.
    */
   template<class FG>
-  void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
+  void augment(const FG& factors, const FactorIndices* newFactorIndices = nullptr);
+
+  /**
+   * An overload of augment() that takes a single factor. and l-value
+   * reference to FactorIndeces.
+   */
+  template<class FG>
+  void augment(const FG& factor, const FactorIndices& newFactorIndices) {
+    augment(factor, &newFactorIndices);
+  }
 
   /**
    * Augment the variable index after an existing factor now affects to more

gtsam/linear/IterativeSolver.cpp

@@ -84,8 +84,7 @@ string IterativeOptimizationParameters::verbosityTranslator(
 
 /*****************************************************************************/
 VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
-    boost::optional<const KeyInfo&> keyInfo,
-    boost::optional<const std::map<Key, Vector>&> lambda) {
+    const KeyInfo* keyInfo, const std::map<Key, Vector>* lambda) {
   return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
       lambda ? *lambda : std::map<Key, Vector>());
 }

gtsam/linear/IterativeSolver.h

@@ -93,8 +93,8 @@ class IterativeSolver {
 
   /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
   GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
-      boost::optional<const KeyInfo&> = boost::none,
-      boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
+      const KeyInfo* = nullptr,
+      const std::map<Key, Vector>* lambda = nullptr);
 
   /* interface to the nonlinear optimizer, without initial estimate */
   GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,

gtsam/linear/linearAlgorithms-inst.h

@@ -21,9 +21,10 @@
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/base/treeTraversal-inst.h>
 
-#include <boost/optional.hpp>
 #include <boost/shared_ptr.hpp>
 
+#include <optional>
+
 namespace gtsam
 {
   namespace internal
@@ -32,7 +33,7 @@ namespace gtsam
     {
       /* ************************************************************************* */
       struct OptimizeData {
-        boost::optional<OptimizeData&> parentData;
+        OptimizeData* parentData = nullptr;
         FastMap<Key, VectorValues::const_iterator> cliqueResults;
         //VectorValues ancestorResults;
         //VectorValues results;
@@ -55,7 +56,7 @@ namespace gtsam
           OptimizeData& parentData)
         {
           OptimizeData myData;
-          myData.parentData = parentData;
+          myData.parentData = &parentData;
           // Take any ancestor results we'll need
           for(Key parent: clique->conditional_->parents())
             myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));

gtsam/navigation/AHRSFactor.cpp

@@ -119,8 +119,8 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
 
 //------------------------------------------------------------------------------
 Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
-    const Vector3& bias, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
+    const Vector3& bias, OptionalMatrixType H1,
+    OptionalMatrixType H2, OptionalMatrixType H3) const {
 
   // Do bias correction, if (H3) will contain 3*3 derivative used below
   const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);

gtsam/navigation/AHRSFactor.h

@@ -140,6 +140,9 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
 
 public:
 
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
   /** Shorthand for a smart pointer to a factor */
 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
   typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
@@ -179,9 +182,8 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
 
   /// vector of errors
   Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
-      const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
-          boost::none) const override;
+      const Vector3& bias, OptionalMatrixType H1,
+      OptionalMatrixType H2, OptionalMatrixType H3) const override;
 
   /// predicted states from IMU
   /// TODO(frank): relationship with PIM predict ??

gtsam/navigation/AttitudeFactor.h

@@ -82,6 +82,9 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public At
 
 public:
 
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
 
@@ -121,8 +124,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public At
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  Vector evaluateError(const Rot3& nRb, //
-      boost::optional<Matrix&> H = boost::none) const override {
+  Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
     return attitudeError(nRb, H);
   }
 
@@ -157,6 +159,9 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
 
 public:
 
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
 
@@ -196,8 +201,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  Vector evaluateError(const Pose3& nTb, //
-      boost::optional<Matrix&> H = boost::none) const override {
+  Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override {
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;

gtsam/navigation/BarometricFactor.cpp

@@ -43,8 +43,8 @@ bool BarometricFactor::equals(const NonlinearFactor& expected,
 
 //***************************************************************************
 Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
-                                       boost::optional<Matrix&> H,
-                                       boost::optional<Matrix&> H2) const {
+                                       OptionalMatrixType H,
+                                       OptionalMatrixType H2) const {
     Matrix tH;
     Vector ret = (Vector(1) << (p.translation(tH).z() + bias - nT_)).finished();
     if (H) (*H) = tH.block<1, 6>(2, 0);

gtsam/navigation/BarometricFactor.h

@@ -38,6 +38,10 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
     double nT_;  ///< Height Measurement based on a standard atmosphere
 
    public:
+
+    // Provide access to the Matrix& version of evaluateError:
+    using Base::evaluateError;
+
     /// shorthand for a smart pointer to a factor
     typedef boost::shared_ptr<BarometricFactor> shared_ptr;
 
@@ -76,10 +80,8 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
                 double tol = 1e-9) const override;
 
     /// vector of errors
-    Vector evaluateError(
-        const Pose3& p, const double& b,
-        boost::optional<Matrix&> H = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const override;
+    Vector evaluateError(const Pose3& p, const double& b, 
+            OptionalMatrixType H, OptionalMatrixType H2) const override;
 
     inline const double& measurementIn() const { return nT_; }
 

gtsam/navigation/CombinedImuFactor.cpp

@@ -222,9 +222,9 @@ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
 Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
     const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
     const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
-    boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
-    boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
+    OptionalMatrixType H1, OptionalMatrixType H2,
+    OptionalMatrixType H3, OptionalMatrixType H4,
+    OptionalMatrixType H5, OptionalMatrixType H6) const {
 
   // error wrt bias evolution model (random walk)
   Matrix6 Hbias_i, Hbias_j;

gtsam/navigation/CombinedImuFactor.h

@@ -269,6 +269,9 @@ class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, P
 
 public:
 
+  // Provide access to Matrix& version of evaluateError:
+  using Base::evaluateError;
+
   /** Shorthand for a smart pointer to a factor */
 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
   typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
@@ -324,10 +327,9 @@ class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, P
   Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
       const Pose3& pose_j, const Vector3& vel_j,
       const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none, boost::optional<Matrix&> H3 = boost::none,
-      boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
-          boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
+      OptionalMatrixType H1, OptionalMatrixType H2, 
+      OptionalMatrixType H3, OptionalMatrixType H4, 
+      OptionalMatrixType H5, OptionalMatrixType H6) const override;
 
  private:
   /** Serialization function */

gtsam/navigation/ConstantVelocityFactor.h

@@ -30,7 +30,10 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
     double dt_;
 
    public:
-    using Base = NoiseModelFactorN<NavState, NavState>;
+    using Base = NoiseModelFactor2<NavState, NavState>;
+
+    // Provide access to the Matrix& version of evaluateError:
+    using Base::evaluateError;
 
    public:
     ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
@@ -48,8 +51,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
      * @return * Vector
      */
     gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
-                                boost::optional<gtsam::Matrix &> H1 = boost::none,
-                                boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
+                                OptionalMatrixType H1, OptionalMatrixType H2) const override {
         // only used to use update() below
         static const Vector3 b_accel{0.0, 0.0, 0.0};
         static const Vector3 b_omega{0.0, 0.0, 0.0};