Merge pull request #1392 from kartikarcot/verdant/typedef-optional-references
						commit
						c7e18e64e5
					
				|  | @ -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 | ||||
|  |  | |||
|  | @ -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, | ||||
|  |  | |||
|  | @ -46,10 +46,9 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// 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_; | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -94,8 +94,10 @@ public: | |||
|   /// 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()); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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>()); | ||||
| } | ||||
|  |  | |||
|  | @ -93,8 +93,8 @@ public: | |||
| 
 | ||||
|   /* 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, | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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 @@ public: | |||
| 
 | ||||
|   /// 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 ??
 | ||||
|  |  | |||
|  | @ -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 @@ public: | |||
|   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 @@ public: | |||
|   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; | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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_; } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -269,6 +269,9 @@ private: | |||
| 
 | ||||
| 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 @@ public: | |||
|   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 */ | ||||
|  |  | |||
|  | @ -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}; | ||||
|  |  | |||
|  | @ -38,7 +38,7 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { | |||
| 
 | ||||
| //***************************************************************************
 | ||||
| Vector GPSFactor::evaluateError(const Pose3& p, | ||||
|     boost::optional<Matrix&> H) const { | ||||
|     OptionalMatrixType H) const { | ||||
|   return p.translation(H) -nT_; | ||||
| } | ||||
| 
 | ||||
|  | @ -80,7 +80,7 @@ bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { | |||
| 
 | ||||
| //***************************************************************************
 | ||||
| Vector GPSFactor2::evaluateError(const NavState& p, | ||||
|     boost::optional<Matrix&> H) const { | ||||
|     OptionalMatrixType H) const { | ||||
|   return p.position(H) -nT_; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -42,6 +42,9 @@ private: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<GPSFactor> shared_ptr; | ||||
| 
 | ||||
|  | @ -78,8 +81,7 @@ public: | |||
|   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; | ||||
| 
 | ||||
|   /// vector of errors
 | ||||
|   Vector evaluateError(const Pose3& p, | ||||
|       boost::optional<Matrix&> H = boost::none) const override; | ||||
|   Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override; | ||||
| 
 | ||||
|   inline const Point3 & measurementIn() const { | ||||
|     return nT_; | ||||
|  | @ -121,6 +123,9 @@ private: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<GPSFactor2> shared_ptr; | ||||
| 
 | ||||
|  | @ -151,8 +156,7 @@ public: | |||
|   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; | ||||
| 
 | ||||
|   /// vector of errors
 | ||||
|   Vector evaluateError(const NavState& p, | ||||
|       boost::optional<Matrix&> H = boost::none) const override; | ||||
|   Vector evaluateError(const NavState& p, OptionalMatrixType H) const override; | ||||
| 
 | ||||
|   inline const Point3 & measurementIn() const { | ||||
|     return nT_; | ||||
|  |  | |||
|  | @ -151,9 +151,9 @@ bool ImuFactor::equals(const NonlinearFactor& other, double tol) const { | |||
| //------------------------------------------------------------------------------
 | ||||
| Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, | ||||
|     const Pose3& pose_j, const Vector3& vel_j, | ||||
|     const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1, | ||||
|     boost::optional<Matrix&> H2, boost::optional<Matrix&> H3, | ||||
|     boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const { | ||||
|     const imuBias::ConstantBias& bias_i, OptionalMatrixType H1, | ||||
|     OptionalMatrixType H2, OptionalMatrixType H3, | ||||
|     OptionalMatrixType H4, OptionalMatrixType H5) const { | ||||
|   return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, | ||||
|       H1, H2, H3, H4, H5); | ||||
| } | ||||
|  | @ -247,8 +247,8 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { | |||
| Vector ImuFactor2::evaluateError(const NavState& state_i, | ||||
|     const NavState& state_j, | ||||
|     const imuBias::ConstantBias& bias_i, //
 | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, | ||||
|     boost::optional<Matrix&> H3) const { | ||||
|     OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|     OptionalMatrixType H3) const { | ||||
|   return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -180,6 +180,9 @@ private: | |||
| 
 | ||||
| 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<ImuFactor> shared_ptr; | ||||
|  | @ -228,10 +231,8 @@ public: | |||
|   /// vector of errors
 | ||||
|   Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, | ||||
|       const Pose3& pose_j, const Vector3& vel_j, | ||||
|       const imuBias::ConstantBias& bias_i, 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) const override; | ||||
|       const imuBias::ConstantBias& bias_i, OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override; | ||||
| 
 | ||||
| #ifdef GTSAM_TANGENT_PREINTEGRATION | ||||
|   /// Merge two pre-integrated measurement classes
 | ||||
|  | @ -270,6 +271,9 @@ private: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /** Default constructor - only use for serialization */ | ||||
|   ImuFactor2() {} | ||||
| 
 | ||||
|  | @ -307,9 +311,8 @@ public: | |||
|   /// vector of errors
 | ||||
|   Vector evaluateError(const NavState& state_i, const NavState& state_j, | ||||
|                        const imuBias::ConstantBias& bias_i,  //
 | ||||
|                        boost::optional<Matrix&> H1 = boost::none, | ||||
|                        boost::optional<Matrix&> H2 = boost::none, | ||||
|                        boost::optional<Matrix&> H3 = boost::none) const override; | ||||
|                        OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|                        OptionalMatrixType H3) const override; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|  |  | |||
|  | @ -38,6 +38,9 @@ class MagFactor: public NoiseModelFactorN<Rot2> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<Rot2>::evaluateError; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Constructor of factor that estimates nav to body rotation bRn | ||||
|    * @param key of the unknown rotation bRn in the factor graph | ||||
|  | @ -61,7 +64,7 @@ public: | |||
|   } | ||||
| 
 | ||||
|   static Point3 unrotate(const Rot2& R, const Point3& p, | ||||
|       boost::optional<Matrix&> HR = boost::none) { | ||||
|       OptionalMatrixType HR = OptionalNone) { | ||||
|     Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); | ||||
|     if (HR) { | ||||
|       // assign to temporary first to avoid error in Win-Debug mode
 | ||||
|  | @ -74,8 +77,7 @@ public: | |||
|   /**
 | ||||
|    * @brief vector of errors | ||||
|    */ | ||||
|   Vector evaluateError(const Rot2& nRb, | ||||
|       boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const Rot2& nRb, OptionalMatrixType H) const override { | ||||
|     // measured bM = nRb<52> * nM + b
 | ||||
|     Point3 hx = unrotate(nRb, nM_, H) + bias_; | ||||
|     return (hx - measured_); | ||||
|  | @ -95,6 +97,10 @@ class MagFactor1: public NoiseModelFactorN<Rot3> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<Rot3>::evaluateError; | ||||
| 
 | ||||
| 
 | ||||
|   /** Constructor */ | ||||
|   MagFactor1(Key key, const Point3& measured, double scale, | ||||
|       const Unit3& direction, const Point3& bias, | ||||
|  | @ -112,10 +118,9 @@ public: | |||
|   /**
 | ||||
|    * @brief 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 { | ||||
|     // measured bM = nRb<52> * nM + b
 | ||||
|     Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; | ||||
|     Point3 hx = nRb.unrotate(nM_, H, OptionalNone) + bias_; | ||||
|     return (hx - measured_); | ||||
|   } | ||||
| }; | ||||
|  | @ -132,6 +137,10 @@ class MagFactor2: public NoiseModelFactorN<Point3, Point3> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor2<Point3, Point3>::evaluateError; | ||||
| 
 | ||||
| 
 | ||||
|   /** Constructor */ | ||||
|   MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb, | ||||
|       const SharedNoiseModel& model) : | ||||
|  | @ -151,10 +160,9 @@ public: | |||
|    * @param bias (unknown) 3D bias | ||||
|    */ | ||||
|   Vector evaluateError(const Point3& nM, const Point3& bias, | ||||
|       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = | ||||
|           boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     // measured bM = nRb<52> * nM + b, where b is unknown bias
 | ||||
|     Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; | ||||
|     Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias; | ||||
|     if (H2) | ||||
|       *H2 = I_3x3; | ||||
|     return (hx - measured_); | ||||
|  | @ -173,6 +181,10 @@ class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor3<double, Unit3, Point3>::evaluateError; | ||||
| 
 | ||||
| 
 | ||||
|   /** Constructor */ | ||||
|   MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, | ||||
|       const Rot3& nRb, const SharedNoiseModel& model) : | ||||
|  | @ -192,11 +204,10 @@ public: | |||
|    * @param bias (unknown) 3D bias | ||||
|    */ | ||||
|   Vector evaluateError(const double& scale, const Unit3& direction, | ||||
|       const Point3& bias, boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = | ||||
|           boost::none) const override { | ||||
|       const Point3& bias, OptionalMatrixType H1, | ||||
|       OptionalMatrixType H2, OptionalMatrixType H3) const override { | ||||
|     // measured bM = nRb<52> * nM + b, where b is unknown bias
 | ||||
|     Unit3 rotated = bRn_.rotate(direction, boost::none, H2); | ||||
|     Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2); | ||||
|     Point3 hx = scale * rotated.point3() + bias; | ||||
|     if (H1) | ||||
|       *H1 = rotated.point3(); | ||||
|  |  | |||
|  | @ -49,6 +49,10 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> { | |||
|   GTSAM_CONCEPT_POSE_TYPE(POSE) | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   ~MagPoseFactor() override {} | ||||
| 
 | ||||
|   /// Default constructor - only use for serialization.
 | ||||
|  | @ -108,11 +112,11 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> { | |||
|    * Return the factor's error h(x) - z, and the optional Jacobian. Note that | ||||
|    * the measurement error is expressed in the body frame. | ||||
|    */ | ||||
|   Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const POSE& nPb, OptionalMatrixType H) const override { | ||||
|     // Predict the measured magnetic field h(x) in the *body* frame.
 | ||||
|     // If body_P_sensor was given, bias_ will have been rotated into the body frame.
 | ||||
|     Matrix H_rot = Matrix::Zero(MeasDim, RotDim); | ||||
|     const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_; | ||||
|     const Point hx = nPb.rotation().unrotate(nM_, H_rot, OptionalNone) + bias_; | ||||
| 
 | ||||
|     if (H) { | ||||
|       // Fill in the relevant part of the Jacobian (just rotation columns).
 | ||||
|  |  | |||
|  | @ -21,6 +21,8 @@ | |||
| #include <gtsam/base/numericalDerivative.h> | ||||
| 
 | ||||
| #include <boost/bind/bind.hpp> | ||||
| #include <functional> | ||||
| #include "gtsam/base/Matrix.h" | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| using namespace std::placeholders; | ||||
|  | @ -47,11 +49,11 @@ TEST( Rot3AttitudeFactor, Constructor ) { | |||
|   Rot3 nRb; | ||||
|   EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); | ||||
| 
 | ||||
|   auto err_fn = [&factor](const Rot3& r){ | ||||
|     return factor.evaluateError(r, OptionalNone); | ||||
|   }; | ||||
|   // Calculate numerical derivatives
 | ||||
|   Matrix expectedH = numericalDerivative11<Vector, Rot3>( | ||||
|       std::bind(&Rot3AttitudeFactor::evaluateError, &factor, | ||||
|                 std::placeholders::_1, boost::none), | ||||
|       nRb); | ||||
|   Matrix expectedH = numericalDerivative11<Vector, Rot3>(err_fn, nRb); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative
 | ||||
|   Matrix actualH; | ||||
|  | @ -98,10 +100,13 @@ TEST( Pose3AttitudeFactor, Constructor ) { | |||
|   Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); | ||||
|   EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5)); | ||||
| 
 | ||||
|   Matrix actualH1; | ||||
| 
 | ||||
|   auto err_fn = [&factor](const Pose3& p){ | ||||
|     return factor.evaluateError(p, OptionalNone); | ||||
|   }; | ||||
|   // Calculate numerical derivatives
 | ||||
|   Matrix expectedH = numericalDerivative11<Vector,Pose3>( | ||||
|       std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1, | ||||
|           boost::none), T); | ||||
|   Matrix expectedH = numericalDerivative11<Vector,Pose3>(err_fn, T); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative
 | ||||
|   Matrix actualH; | ||||
|  |  | |||
|  | @ -58,15 +58,11 @@ TEST(BarometricFactor, Constructor) { | |||
| 
 | ||||
|     // Calculate numerical derivatives
 | ||||
|     Matrix expectedH = numericalDerivative21<Vector, Pose3, double>( | ||||
|         std::bind(&BarometricFactor::evaluateError, &factor, | ||||
|                   std::placeholders::_1, std::placeholders::_2, boost::none, | ||||
|                   boost::none), | ||||
|         T, baroBias); | ||||
|         [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);}, | ||||
| 		T, baroBias); | ||||
| 
 | ||||
|     Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>( | ||||
|         std::bind(&BarometricFactor::evaluateError, &factor, | ||||
|                   std::placeholders::_1, std::placeholders::_2, boost::none, | ||||
|                   boost::none), | ||||
|         [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);}, | ||||
|         T, baroBias); | ||||
| 
 | ||||
|     // Use the factor to calculate the derivative
 | ||||
|  | @ -99,15 +95,11 @@ TEST(BarometricFactor, nonZero) { | |||
| 
 | ||||
|     // Calculate numerical derivatives
 | ||||
|     Matrix expectedH = numericalDerivative21<Vector, Pose3, double>( | ||||
|         std::bind(&BarometricFactor::evaluateError, &factor, | ||||
|                   std::placeholders::_1, std::placeholders::_2, boost::none, | ||||
|                   boost::none), | ||||
|         [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);}, | ||||
|         T, baroBias); | ||||
| 
 | ||||
|     Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>( | ||||
|         std::bind(&BarometricFactor::evaluateError, &factor, | ||||
|                   std::placeholders::_1, std::placeholders::_2, boost::none, | ||||
|                   boost::none), | ||||
|         [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);}, | ||||
|         T, baroBias); | ||||
| 
 | ||||
|     // Use the factor to calculate the derivative and the error
 | ||||
|  |  | |||
|  | @ -20,8 +20,6 @@ | |||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| 
 | ||||
| #include <boost/bind/bind.hpp> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <GeographicLib/Config.h> | ||||
|  | @ -71,7 +69,7 @@ TEST( GPSFactor, Constructor ) { | |||
| 
 | ||||
|   // Calculate numerical derivatives
 | ||||
|   Matrix expectedH = numericalDerivative11<Vector, Pose3>( | ||||
|       std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); | ||||
|       [&factor](const Pose3& T) { return factor.evaluateError(T); }, T); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative
 | ||||
|   Matrix actualH; | ||||
|  | @ -100,7 +98,7 @@ TEST( GPSFactor2, Constructor ) { | |||
| 
 | ||||
|   // Calculate numerical derivatives
 | ||||
|   Matrix expectedH = numericalDerivative11<Vector, NavState>( | ||||
|       std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T); | ||||
|       [&factor](const NavState& T) { return factor.evaluateError(T); }, T); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative
 | ||||
|   Matrix actualH; | ||||
|  |  | |||
|  | @ -56,15 +56,13 @@ Unit3 dir(nM); | |||
| SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); | ||||
| }  // namespace
 | ||||
| 
 | ||||
| using boost::none; | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
| TEST( MagFactor, unrotate ) { | ||||
|   Matrix H; | ||||
|   Point3 expected(22735.5, 314.502, 44202.5); | ||||
|   EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); | ||||
|   EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,&H),1e-1)); | ||||
|   EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
 | ||||
|       (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); | ||||
|       (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, OptionalNone), theta), H, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
|  | @ -75,37 +73,32 @@ TEST( MagFactor, Factors ) { | |||
|   // MagFactor
 | ||||
|   MagFactor f(1, measured, s, dir, bias, model); | ||||
|   EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); | ||||
|   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
 | ||||
|       (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); | ||||
|   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> | ||||
|         ([&f] (const Rot2& theta) { return f.evaluateError(theta); }, theta), H1, 1e-7)); | ||||
| 
 | ||||
|   // MagFactor1
 | ||||
|   MagFactor1 f1(1, measured, s, dir, bias, model); | ||||
|   EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); | ||||
|   EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
 | ||||
|       (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); | ||||
|   EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> | ||||
|       ([&f1] (const Rot3& nRb) { return f1.evaluateError(nRb); }, nRb), H1, 1e-7)); | ||||
| 
 | ||||
|   // MagFactor2
 | ||||
|   MagFactor2 f2(1, 2, measured, nRb, model); | ||||
|   EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); | ||||
|   EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
 | ||||
|       (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
 | ||||
|       H1, 1e-7)); | ||||
|       ([&f2] (const Point3& scaled) { return f2.evaluateError(scaled,bias); }, scaled), H1, 1e-7)); | ||||
|   EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
 | ||||
|       (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
 | ||||
|       H2, 1e-7)); | ||||
|       ([&f2] (const Point3& bias) { return f2.evaluateError(scaled,bias); }, bias), H2, 1e-7)); | ||||
| 
 | ||||
|   // MagFactor3
 | ||||
|   MagFactor3 f3(1, 2, 3, measured, nRb, model); | ||||
|   EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); | ||||
|   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
 | ||||
|       (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),//
 | ||||
|       H1, 1e-7)); | ||||
|       ([&f3] (double s) { return f3.evaluateError(s,dir,bias); }, s), H1, 1e-7)); | ||||
|   EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
 | ||||
|       (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),//
 | ||||
|       H2, 1e-7)); | ||||
|       ([&f3] (const Unit3& dir) { return f3.evaluateError(s,dir,bias); }, dir), H2, 1e-7)); | ||||
|   EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
 | ||||
|       (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),//
 | ||||
|       H3, 1e-7)); | ||||
|       ([&f3] (const Point3& bias) { return f3.evaluateError(s,dir,bias); }, bias), H3, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
|  |  | |||
|  | @ -78,8 +78,7 @@ TEST(MagPoseFactor, JacobianPose2) { | |||
|   MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); | ||||
|   CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); | ||||
|   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2>  //
 | ||||
|                             (std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, | ||||
|                                        std::placeholders::_1, boost::none), | ||||
|                             ([&f] (const Pose2& p) {return f.evaluateError(p);}, | ||||
|                              n_P2_b), | ||||
|                             H2, 1e-7)); | ||||
| } | ||||
|  | @ -92,8 +91,7 @@ TEST(MagPoseFactor, JacobianPose3) { | |||
|   MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); | ||||
|   CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); | ||||
|   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3>  //
 | ||||
|                             (std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, | ||||
|                                        std::placeholders::_1, boost::none), | ||||
|                             ([&f] (const Pose3& p) {return f.evaluateError(p);}, | ||||
|                              n_P3_b), | ||||
|                             H3, 1e-7)); | ||||
| } | ||||
|  | @ -109,7 +107,7 @@ TEST(MagPoseFactor, body_P_sensor2) { | |||
|   MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor); | ||||
|   CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); | ||||
|   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
 | ||||
|       (std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7)); | ||||
|       ([&f] (const Pose2& p) {return f.evaluateError(p);},n_P2_b), H2, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| // *****************************************************************************
 | ||||
|  | @ -123,7 +121,7 @@ TEST(MagPoseFactor, body_P_sensor3) { | |||
|   MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor); | ||||
|   CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); | ||||
|   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
 | ||||
|       (std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7)); | ||||
|       ([&f] (const Pose3& p) {return f.evaluateError(p);}, n_P3_b), H3, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| // *****************************************************************************
 | ||||
|  |  | |||
|  | @ -22,7 +22,7 @@ namespace gtsam { | |||
| /*
 | ||||
|  * Calculates the unwhitened error by invoking the callback functor (i.e. from Python). | ||||
|  */ | ||||
| Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const { | ||||
| Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { | ||||
|   if(this->active(x)) { | ||||
| 
 | ||||
|     if(H) { | ||||
|  | @ -43,7 +43,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vecto | |||
|        *    return error | ||||
|        * ``` | ||||
|        */ | ||||
|       return this->error_function_(*this, x, H.get_ptr()); | ||||
|       return this->error_function_(*this, x, H); | ||||
|     } else { | ||||
|       /*
 | ||||
|        * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python. | ||||
|  |  | |||
|  | @ -75,7 +75,7 @@ public: | |||
|     * Calls the errorFunction closure, which is a std::function object | ||||
|     * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array | ||||
|     */ | ||||
|   Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override; | ||||
|   Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = nullptr) const override; | ||||
| 
 | ||||
|   /** print */ | ||||
|   void print(const std::string &s, | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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 = | ||||
|       boost::none) const; | ||||
|   T value(const Values& values, std::vector<Matrix>* H = nullptr) 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 | ||||
|  |  | |||
|  | @ -40,8 +40,8 @@ namespace gtsam { | |||
|  * \tparam T Type for measurements. | ||||
|  * | ||||
|  */ | ||||
| template<typename T> | ||||
| class ExpressionFactor: public NoiseModelFactor { | ||||
| template <typename T> | ||||
| class ExpressionFactor : public NoiseModelFactor { | ||||
|   BOOST_CONCEPT_ASSERT((IsTestable<T>)); | ||||
| 
 | ||||
| protected: | ||||
|  | @ -55,6 +55,9 @@ protected: | |||
| 
 | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of unwhitenedError:
 | ||||
|   using NoiseModelFactor::unwhitenedError; | ||||
|   typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr; | ||||
| 
 | ||||
|   /**
 | ||||
|  | @ -97,7 +100,7 @@ protected: | |||
|    * both the function evaluation and its derivative(s) in H. | ||||
|    */ | ||||
|   Vector unwhitenedError(const Values& x, | ||||
|     boost::optional<std::vector<Matrix>&> H = boost::none) const override { | ||||
|     OptionalMatrixVecType H = nullptr) const override { | ||||
|     if (H) { | ||||
|       const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); | ||||
|       // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
 | ||||
|  | @ -243,6 +246,9 @@ class ExpressionFactorN : public ExpressionFactor<T> { | |||
| public: | ||||
|   static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); | ||||
|   using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>; | ||||
|      | ||||
|   // Provide access to the Matrix& version of unwhitenedError:
 | ||||
|   using ExpressionFactor<T>::unwhitenedError; | ||||
| 
 | ||||
|   /// Destructor
 | ||||
|   ~ExpressionFactorN() override = default; | ||||
|  | @ -311,9 +317,8 @@ public: | |||
|   ~ExpressionFactor2() override {} | ||||
| 
 | ||||
|   /// Backwards compatible evaluateError, to make existing tests compile
 | ||||
|   Vector evaluateError(const A1 &a1, const A2 &a2, | ||||
|                        boost::optional<Matrix &> H1 = boost::none, | ||||
|                        boost::optional<Matrix &> H2 = boost::none) const { | ||||
|   Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1 = OptionalNone, | ||||
|                                OptionalMatrixType H2 = OptionalNone) const { | ||||
|     Values values; | ||||
|     values.insert(this->keys_[0], a1); | ||||
|     values.insert(this->keys_[1], a2); | ||||
|  | @ -324,15 +329,13 @@ public: | |||
|     return error; | ||||
|   } | ||||
| 
 | ||||
| 
 | ||||
|   /// Recreate expression from given keys_ and measured_, used in load
 | ||||
|   /// Needed to deserialize a derived factor
 | ||||
|   virtual Expression<T> expression(Key key1, Key key2) const { | ||||
|     throw std::runtime_error( | ||||
|         "ExpressionFactor2::expression not provided: cannot deserialize."); | ||||
|     throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize."); | ||||
|   } | ||||
|   Expression<T> | ||||
|   expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys) | ||||
|       const override { | ||||
|   Expression<T> expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys& keys) const override { | ||||
|     return expression(keys[0], keys[1]); | ||||
|   } | ||||
| 
 | ||||
|  | @ -341,7 +344,7 @@ protected: | |||
|   ExpressionFactor2() {} | ||||
| 
 | ||||
|   /// Constructor takes care of keys, but still need to call initialize
 | ||||
|   ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel, | ||||
|   ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel& noiseModel, | ||||
|                     const T &measurement) | ||||
|       : ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {} | ||||
| }; | ||||
|  |  | |||
|  | @ -62,9 +62,13 @@ class FunctorizedFactor : public NoiseModelFactorN<T> { | |||
| 
 | ||||
|   R measured_;  ///< value that is compared with functor return value
 | ||||
|   SharedNoiseModel noiseModel_;                          ///< noise model
 | ||||
|   std::function<R(T, boost::optional<Matrix &>)> func_;  ///< functor instance
 | ||||
|   std::function<R(T, OptionalMatrixType)> func_;  ///< functor instance
 | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /** default constructor - only use for serialization */ | ||||
|   FunctorizedFactor() {} | ||||
| 
 | ||||
|  | @ -76,7 +80,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> { | |||
|    * @param func: The instance of the functor object | ||||
|    */ | ||||
|   FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, | ||||
|                     const std::function<R(T, boost::optional<Matrix &>)> func) | ||||
|                     const std::function<R(T, OptionalMatrixType)> func) | ||||
|       : Base(model, key), measured_(z), noiseModel_(model), func_(func) {} | ||||
| 
 | ||||
|   ~FunctorizedFactor() override {} | ||||
|  | @ -87,8 +91,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> { | |||
|         NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this))); | ||||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const T ¶ms, boost::optional<Matrix &> H = | ||||
|                                             boost::none) const override { | ||||
|   Vector evaluateError(const T ¶ms, OptionalMatrixType H) const override { | ||||
|     R x = func_(params, H); | ||||
|     Vector error = traits<R>::Local(measured_, x); | ||||
|     return error; | ||||
|  | @ -162,11 +165,14 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> { | |||
| 
 | ||||
|   R measured_;  ///< value that is compared with functor return value
 | ||||
|   SharedNoiseModel noiseModel_;  ///< noise model
 | ||||
|   using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>, | ||||
|                                        boost::optional<Matrix &>)>; | ||||
|   using FunctionType = std::function<R(T1, T2, OptionalMatrixType, OptionalMatrixType)>; | ||||
|   FunctionType func_;  ///< functor instance
 | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /** default constructor - only use for serialization */ | ||||
|   FunctorizedFactor2() {} | ||||
| 
 | ||||
|  | @ -194,8 +200,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> { | |||
| 
 | ||||
|   Vector evaluateError( | ||||
|       const T1 ¶ms1, const T2 ¶ms2, | ||||
|       boost::optional<Matrix &> H1 = boost::none, | ||||
|       boost::optional<Matrix &> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     R x = func_(params1, params2, H1, H2); | ||||
|     Vector error = traits<R>::Local(measured_, x); | ||||
|     return error; | ||||
|  |  | |||
|  | @ -478,8 +478,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, | |||
| /* ************************************************************************* */ | ||||
| void ISAM2::marginalizeLeaves( | ||||
|     const FastList<Key>& leafKeysList, | ||||
|     boost::optional<FactorIndices&> marginalFactorsIndices, | ||||
|     boost::optional<FactorIndices&> deletedFactorsIndices) { | ||||
|     FactorIndices* marginalFactorsIndices, | ||||
|     FactorIndices* deletedFactorsIndices) { | ||||
|   // Convert to ordered set
 | ||||
|   KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); | ||||
| 
 | ||||
|  |  | |||
|  | @ -198,8 +198,20 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> { | |||
|    */ | ||||
|   void marginalizeLeaves( | ||||
|       const FastList<Key>& leafKeys, | ||||
|       boost::optional<FactorIndices&> marginalFactorsIndices = boost::none, | ||||
|       boost::optional<FactorIndices&> deletedFactorsIndices = boost::none); | ||||
|       FactorIndices* marginalFactorsIndices = nullptr, | ||||
|       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_; } | ||||
|  |  | |||
|  | @ -47,6 +47,9 @@ class NonlinearEquality: public NoiseModelFactorN<VALUE> { | |||
| public: | ||||
|   typedef VALUE T; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<VALUE>::evaluateError; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   // feasible value
 | ||||
|  | @ -138,8 +141,7 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// Error function
 | ||||
|   Vector evaluateError(const T& xj, | ||||
|       boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const T& xj, OptionalMatrixType H) const override { | ||||
|     const size_t nj = traits<T>::GetDimension(feasible_); | ||||
|     if (allow_error_) { | ||||
|       if (H) | ||||
|  | @ -209,6 +211,9 @@ class NonlinearEquality1: public NoiseModelFactorN<VALUE> { | |||
| public: | ||||
|   typedef VALUE X; | ||||
| 
 | ||||
|   // Provide access to Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<VALUE>::evaluateError; | ||||
| 
 | ||||
| protected: | ||||
|   typedef NoiseModelFactorN<VALUE> Base; | ||||
|   typedef NonlinearEquality1<VALUE> This; | ||||
|  | @ -248,8 +253,7 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// g(x) with optional derivative
 | ||||
|   Vector evaluateError(const X& x1, | ||||
|       boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const X& x1, OptionalMatrixType H) const override { | ||||
|     if (H) | ||||
|       (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1)); | ||||
|     // manifold equivalent of h(x)-z -> log(z,h(x))
 | ||||
|  | @ -305,6 +309,10 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> { | |||
|  public: | ||||
|   typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
| 
 | ||||
|   /**
 | ||||
|    * Constructor | ||||
|    * @param key1 the key for the first unknown variable to be constrained | ||||
|  | @ -324,8 +332,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> { | |||
| 
 | ||||
|   /// g(x) with optional derivative2
 | ||||
|   Vector evaluateError( | ||||
|       const T& x1, const T& x2, boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     static const size_t p = traits<T>::dimension; | ||||
|     if (H1) *H1 = -Matrix::Identity(p, p); | ||||
|     if (H2) *H2 = Matrix::Identity(p, p); | ||||
|  |  | |||
|  | @ -29,11 +29,35 @@ | |||
| #include <gtsam/base/utilities.h>  // boost::index_sequence | ||||
| 
 | ||||
| #include <boost/serialization/base_object.hpp> | ||||
| #include <cstddef> | ||||
| #include <type_traits> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| /** These typedefs and aliases will help with making the evaluateError interface
 | ||||
|  * independent of boost | ||||
|  * TODO(kartikarcot): Change this to OptionalMatrixNone | ||||
|  * This typedef is used to indicate that the Jacobian is not required | ||||
|  * and the default value used for optional matrix pointer arguments in evaluateError. | ||||
|  * Had to use the static_cast of a nullptr, because the compiler is not able to | ||||
|  * deduce the type of the nullptr when expanding the evaluateError templates. | ||||
|  */ | ||||
| #define OptionalNone static_cast<Matrix*>(nullptr) | ||||
| 
 | ||||
| /** This typedef will be used everywhere boost::optional<Matrix&> reference was used
 | ||||
|  * previously. This is used to indicate that the Jacobian is optional. In the future | ||||
|  * we will change this to OptionalJacobian | ||||
|  */ | ||||
| using OptionalMatrixType = Matrix*; | ||||
| 
 | ||||
| /** The OptionalMatrixVecType is a pointer to a vector of matrices. It will
 | ||||
|  * be used in situations where a vector of matrices is optional, like in  | ||||
|  * unwhitenedError. | ||||
|  */ | ||||
| using OptionalMatrixVecType = std::vector<Matrix>*; | ||||
| 
 | ||||
| /**
 | ||||
|  * Nonlinear factor base class | ||||
|  * | ||||
|  | @ -206,7 +230,6 @@ protected: | |||
|   NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {} | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /** Print */ | ||||
|   void print(const std::string& s = "", | ||||
|     const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; | ||||
|  | @ -230,8 +253,17 @@ public: | |||
|    * If the optional arguments is specified, it should compute | ||||
|    * both the function evaluation and its derivative(s) in H. | ||||
|    */ | ||||
|   virtual Vector unwhitenedError(const Values& x, | ||||
|       boost::optional<std::vector<Matrix>&> H = boost::none) const = 0; | ||||
|   virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; | ||||
| 
 | ||||
|   /** support taking in the actual vector instead of the pointer as well
 | ||||
|    * to get access to this version of the function from derived classes | ||||
|    * one will need to use the "using" keyword and specify that like this: | ||||
|    * public: | ||||
|    *   using NoiseModelFactor::unwhitenedError; | ||||
|    */ | ||||
|   Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const { | ||||
|     return unwhitenedError(x, &H); | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * Vector of errors, whitened | ||||
|  | @ -359,8 +391,8 @@ struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> { | |||
|  * | ||||
|  *   Vector evaluateError( | ||||
|  *       const Pose3& T, const Point3& p, | ||||
|  *       boost::optional<Matrix&> H_T = boost::none, | ||||
|  *       boost::optional<Matrix&> H_p = boost::none) const override { | ||||
|  *       OptionalMatrixType H_T = OptionalNone, | ||||
|  *       OptionalMatrixType H_p = OptionalNone) const override { | ||||
|  *     Matrix36 t_H_T;  // partial derivative of translation w.r.t. pose T
 | ||||
|  * | ||||
|  *     // Only compute t_H_T if needed:
 | ||||
|  | @ -402,7 +434,9 @@ class NoiseModelFactorN | |||
|   /// N is the number of variables (N-way factor)
 | ||||
|   enum { N = sizeof...(ValueTypes) }; | ||||
| 
 | ||||
|  protected: | ||||
|   using NoiseModelFactor::unwhitenedError; | ||||
| 
 | ||||
| protected: | ||||
|   using Base = NoiseModelFactor; | ||||
|   using This = NoiseModelFactorN<ValueTypes...>; | ||||
| 
 | ||||
|  | @ -423,20 +457,49 @@ class NoiseModelFactorN | |||
|   template <typename Container> | ||||
|   using IsContainerOfKeys = IsConvertible<ContainerElementType<Container>, Key>; | ||||
| 
 | ||||
|   /** A helper alias to check if a list of args
 | ||||
|    * are all references to a matrix or not. It will be used | ||||
|    * to choose the right overload of evaluateError. | ||||
|    */ | ||||
|   template <typename Ret, typename ...Args> | ||||
|   using AreAllMatrixRefs = std::enable_if_t<(... &&  | ||||
|       std::is_convertible<Args, Matrix&>::value), Ret>; | ||||
|    | ||||
|   template<typename Arg> | ||||
|   using IsMatrixPointer = std::is_same<typename std::decay_t<Arg>, Matrix*>; | ||||
| 
 | ||||
|   template<typename Arg> | ||||
|   using IsNullpointer = std::is_same<typename std::decay_t<Arg>, std::nullptr_t>; | ||||
| 
 | ||||
|   /** A helper alias to check if a list of args
 | ||||
|    * are all pointers to a matrix or not. It will be used | ||||
|    * to choose the right overload of evaluateError. | ||||
|    */ | ||||
|   template <typename Ret, typename ...Args> | ||||
|     using AreAllMatrixPtrs = std::enable_if_t<(... && | ||||
|             (IsMatrixPointer<Args>::value || IsNullpointer<Args>::value)), Ret>; | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
 | ||||
|   /* Like std::void_t, except produces `OptionalMatrixType` instead of
 | ||||
|    * `void`. Used to expand fixed-type parameter-packs with same length as | ||||
|    * ValueTypes. */ | ||||
|   template <typename T> | ||||
|   using OptionalMatrix = boost::optional<Matrix&>; | ||||
|   template <typename T = void> | ||||
|   using OptionalMatrixTypeT = Matrix*; | ||||
| 
 | ||||
|   /* Like std::void_t, except produces `Key` instead of `void`. Used to expand
 | ||||
|    * fixed-type parameter-packs with same length as ValueTypes. */ | ||||
|   template <typename T> | ||||
|   using KeyType = Key; | ||||
| 
 | ||||
|  public: | ||||
|   /* Like std::void_t, except produces `Matrix` instead of
 | ||||
|    * `void`. Used to expand fixed-type parameter-packs with same length as | ||||
|    * ValueTypes. This helps in creating an evaluateError overload that accepts | ||||
|    * Matrices instead of pointers to matrices */ | ||||
|   template <typename T = void> | ||||
|   using MatrixTypeT = Matrix; | ||||
| 
 | ||||
|   public: | ||||
|   /**
 | ||||
|    * The type of the I'th template param can be obtained as ValueType<I>. | ||||
|    * I is 1-indexed for backwards compatibility/consistency!  So for example, | ||||
|  | @ -541,7 +604,7 @@ class NoiseModelFactorN | |||
|    */ | ||||
|   Vector unwhitenedError( | ||||
|       const Values& x, | ||||
|       boost::optional<std::vector<Matrix>&> H = boost::none) const override { | ||||
|       OptionalMatrixVecType H = nullptr) const override { | ||||
|     return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x, | ||||
|                            H); | ||||
|   } | ||||
|  | @ -560,8 +623,8 @@ class NoiseModelFactorN | |||
|    * ``` | ||||
|    * Vector evaluateError( | ||||
|    *     const Pose3& x1, const Point3& x2, | ||||
|    *     boost::optional<Matrix&> H1 = boost::none, | ||||
|    *     boost::optional<Matrix&> H2 = boost::none) const override { ... } | ||||
|    *     OptionalMatrixType H1 = OptionalNone, | ||||
|    *     OptionalMatrixType H2 = OptionalNone) const override { ... } | ||||
|    * ``` | ||||
|    * | ||||
|    * If any of the optional Matrix reference arguments are specified, it should | ||||
|  | @ -573,10 +636,20 @@ class NoiseModelFactorN | |||
|    * @param[out] H The Jacobian with respect to each variable (optional). | ||||
|    */ | ||||
|   virtual Vector evaluateError(const ValueTypes&... x, | ||||
|                                OptionalMatrix<ValueTypes>... H) const = 0; | ||||
|                                OptionalMatrixTypeT<ValueTypes>... H) const = 0; | ||||
| 
 | ||||
|   /** If all the optional arguments are matrices then redirect the call to 
 | ||||
|    * the one which takes pointers. | ||||
|    * To get access to this version of the function from derived classes | ||||
|    * one will need to use the "using" keyword and specify that like this: | ||||
|    * public: | ||||
|    *   using NoiseModelFactorN<list the value types here>::evaluateError; | ||||
|    */ | ||||
|   Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const { | ||||
|     return evaluateError(x..., (&H)...); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Convenience method overloads
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  | @ -587,19 +660,31 @@ class NoiseModelFactorN | |||
|    * e.g. `const Vector error = factor.evaluateError(pose, point);` | ||||
|    */ | ||||
|   inline Vector evaluateError(const ValueTypes&... x) const { | ||||
|     return evaluateError(x..., OptionalMatrix<ValueTypes>()...); | ||||
|     return evaluateError(x..., OptionalMatrixTypeT<ValueTypes>()...); | ||||
|   } | ||||
| 
 | ||||
|   /** Some (but not all) optional Jacobians are omitted (function overload)
 | ||||
|    * | ||||
|    * and the jacobians are l-value references to matrices. | ||||
|    * e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);` | ||||
|    */ | ||||
|   template <typename... OptionalJacArgs, | ||||
|             typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>> | ||||
|   inline Vector evaluateError(const ValueTypes&... x, | ||||
|                               OptionalJacArgs&&... H) const { | ||||
|     return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., | ||||
|                          boost::none); | ||||
|   template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>> | ||||
|   inline AreAllMatrixRefs<Vector, OptionalJacArgs...> evaluateError(const ValueTypes&... x, | ||||
|                                                                   OptionalJacArgs&&... H) const { | ||||
|     return evaluateError(x..., (&H)...); | ||||
|   } | ||||
| 
 | ||||
|   /** Some (but not all) optional Jacobians are omitted (function overload)
 | ||||
|    * and the jacobians are pointers to matrices. | ||||
|    * e.g. `const Vector error = factor.evaluateError(pose, point, &Hpose);` | ||||
|    */ | ||||
|   template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>> | ||||
|   inline AreAllMatrixPtrs<Vector, OptionalJacArgs...> evaluateError(const ValueTypes&... x, | ||||
|                                                                     OptionalJacArgs&&... H) const { | ||||
|     // If they are pointer version, ensure to cast them all to be Matrix* types
 | ||||
|     // This will ensure any arguments inferred as std::nonetype_t are cast to (Matrix*) nullptr
 | ||||
|     // This guides the compiler to the correct overload which is the one that takes pointers
 | ||||
|     return evaluateError(x...,  | ||||
|         std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone)); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|  | @ -615,7 +700,7 @@ class NoiseModelFactorN | |||
|   inline Vector unwhitenedError( | ||||
|       boost::mp11::index_sequence<Indices...>,  //
 | ||||
|       const Values& x, | ||||
|       boost::optional<std::vector<Matrix>&> H = boost::none) const { | ||||
|       OptionalMatrixVecType H = nullptr) const { | ||||
|     if (this->active(x)) { | ||||
|       if (H) { | ||||
|         return evaluateError(x.at<ValueTypes>(keys_[Indices])..., | ||||
|  |  | |||
|  | @ -32,6 +32,10 @@ namespace gtsam { | |||
|   public: | ||||
|     typedef VALUE T; | ||||
| 
 | ||||
|     // Provide access to the Matrix& version of evaluateError:
 | ||||
|     using NoiseModelFactor1<VALUE>::evaluateError; | ||||
| 
 | ||||
| 
 | ||||
|   private: | ||||
| 
 | ||||
|     typedef NoiseModelFactorN<VALUE> Base; | ||||
|  | @ -91,7 +95,7 @@ namespace gtsam { | |||
|     /** implement functions needed to derive from Factor */ | ||||
| 
 | ||||
|     /** vector of errors */ | ||||
|     Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override { | ||||
|     Vector evaluateError(const T& x, OptionalMatrixType H) const override { | ||||
|       if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x)); | ||||
|       // manifold equivalent of z-x -> Local(x,z)
 | ||||
|       return -traits<T>::Local(x, prior_); | ||||
|  | @ -111,10 +115,10 @@ namespace gtsam { | |||
|       ar & BOOST_SERIALIZATION_NVP(prior_); | ||||
|     } | ||||
| 
 | ||||
| 	// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 | ||||
| 	enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; | ||||
|   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 | ||||
|   enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; | ||||
|   public: | ||||
| 	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) | ||||
|   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) | ||||
|   }; | ||||
| 
 | ||||
|   /// traits
 | ||||
|  |  | |||
|  | @ -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; | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|      * \c Value if the key does exist, or boost::none if it does not exist. | ||||
|     /** 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 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. */ | ||||
|  |  | |||
|  | @ -160,7 +160,7 @@ TEST(FunctorizedFactor, Functional) { | |||
|   Matrix X = Matrix::Identity(3, 3); | ||||
|   Matrix measurement = multiplier * Matrix::Identity(3, 3); | ||||
| 
 | ||||
|   std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional = | ||||
|   std::function<Matrix(Matrix, OptionalMatrixType)> functional = | ||||
|       MultiplyFunctor(multiplier); | ||||
|   auto factor = | ||||
|       MakeFunctorizedFactor<Matrix>(key, measurement, model, functional); | ||||
|  | @ -233,8 +233,7 @@ TEST(FunctorizedFactor, Functional2) { | |||
|   Vector3 x(1, 2, 3); | ||||
|   Vector measurement = A * x; | ||||
| 
 | ||||
|   std::function<Matrix(Matrix, Matrix, boost::optional<Matrix &>, | ||||
|                        boost::optional<Matrix &>)> | ||||
|   std::function<Matrix(Matrix, Matrix, OptionalMatrixType, OptionalMatrixType)> | ||||
|       functional = ProjectionFunctor(); | ||||
|   auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement, | ||||
|                                                        model2, functional); | ||||
|  |  | |||
|  | @ -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); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -62,9 +62,7 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> { | |||
|   } | ||||
|    | ||||
|   Vector evaluateError(const A1& a1, const A2& a2, | ||||
|     boost::optional<Matrix&> H1 = boost::none, | ||||
|     boost::optional<Matrix&> H2 = boost::none) const | ||||
|   { | ||||
|     OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const { | ||||
|     std::vector<Matrix> Hs(2); | ||||
|     const auto &keys = Factor::keys(); | ||||
|     const Vector error = unwhitenedError( | ||||
|  |  | |||
|  | @ -74,9 +74,7 @@ class BearingRangeFactor | |||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const A1& a1, const A2& a2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const | ||||
|   { | ||||
|       OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const { | ||||
|     std::vector<Matrix> Hs(2); | ||||
|     const auto &keys = Factor::keys(); | ||||
|     const Vector error = this->unwhitenedError( | ||||
|  |  | |||
|  | @ -58,16 +58,15 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> { | |||
|     Expression<A2> a2_(keys[1]); | ||||
|     return Expression<T>(Range<A1, A2>(), a1_, a2_); | ||||
|   } | ||||
|    | ||||
|   Vector evaluateError(const A1& a1, const A2& a2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const | ||||
|   { | ||||
| 
 | ||||
|   Vector evaluateError(const A1& a1, const A2& a2,  | ||||
|       OptionalMatrixType H1 = OptionalNone, | ||||
|       OptionalMatrixType H2 = OptionalNone) const { | ||||
|     std::vector<Matrix> Hs(2); | ||||
|     const auto &keys = Factor::keys(); | ||||
|     const auto& keys = Factor::keys(); | ||||
|     const Vector error = Base::unwhitenedError( | ||||
|       {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},  | ||||
|       Hs); | ||||
|         {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, | ||||
|         Hs); | ||||
|     if (H1) *H1 = Hs[0]; | ||||
|     if (H2) *H2 = Hs[1]; | ||||
|     return error; | ||||
|  | @ -137,9 +136,7 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> { | |||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const A1& a1, const A2& a2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const | ||||
|   { | ||||
|       OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const { | ||||
|     std::vector<Matrix> Hs(2); | ||||
|     const auto &keys = Factor::keys(); | ||||
|     const Vector error = Base::unwhitenedError( | ||||
|  | @ -150,6 +147,12 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> { | |||
|     return error; | ||||
|   } | ||||
| 
 | ||||
|   // An evaluateError overload to accept matrices (Matrix&) and pass it to the
 | ||||
|   // OptionalMatrixType evaluateError overload
 | ||||
|   Vector evaluateError(const A1& a1, const A2& a2, Matrix& H1, Matrix& H2) const { | ||||
| 	return evaluateError(a1, a2, &H1, &H2); | ||||
|   } | ||||
| 
 | ||||
|   /** print contents */ | ||||
|   void print(const std::string& s = "", | ||||
|              const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { | ||||
|  |  | |||
|  | @ -214,7 +214,7 @@ TEST( RangeFactor, Jacobian2D ) { | |||
| 
 | ||||
|   // Use the factor to calculate the Jacobians
 | ||||
|   Matrix H1Actual, H2Actual; | ||||
|   factor.evaluateError(pose, point, H1Actual, H2Actual); | ||||
|   factor.evaluateError(pose, point, &H1Actual, &H2Actual); | ||||
| 
 | ||||
|   // Use numerical derivatives to calculate the Jacobians
 | ||||
|   Matrix H1Expected, H2Expected; | ||||
|  |  | |||
|  | @ -75,8 +75,8 @@ bool ShonanFactor<d>::equals(const NonlinearFactor &expected, | |||
| //******************************************************************************
 | ||||
| template <size_t d> | ||||
| void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2, | ||||
|                                     boost::optional<Matrix &> H1, | ||||
|                                     boost::optional<Matrix &> H2) const { | ||||
|                                     OptionalMatrixType H1, | ||||
|                                     OptionalMatrixType H2) const { | ||||
|   gttic(ShonanFactor_Jacobians); | ||||
|   const size_t dim = p_ * d; // Stiefel manifold dimension
 | ||||
| 
 | ||||
|  | @ -106,8 +106,8 @@ void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2, | |||
| //******************************************************************************
 | ||||
| template <size_t d> | ||||
| Vector ShonanFactor<d>::evaluateError(const SOn &Q1, const SOn &Q2, | ||||
|                                       boost::optional<Matrix &> H1, | ||||
|                                       boost::optional<Matrix &> H2) const { | ||||
|                                       OptionalMatrixType H1, | ||||
|                                       OptionalMatrixType H2) const { | ||||
|   gttic(ShonanFactor_evaluateError); | ||||
| 
 | ||||
|   const Matrix &M1 = Q1.matrix(); | ||||
|  |  | |||
|  | @ -42,6 +42,10 @@ class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> { | |||
|   using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor2<SOn, SOn>::evaluateError; | ||||
| 
 | ||||
|   /// @name Constructor
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  | @ -71,17 +75,14 @@ public: | |||
| 
 | ||||
|   /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
 | ||||
|   /// projects down from SO(p) to the Stiefel manifold of px3 matrices.
 | ||||
|   Vector | ||||
|   evaluateError(const SOn &Q1, const SOn &Q2, | ||||
|                 boost::optional<Matrix &> H1 = boost::none, | ||||
|                 boost::optional<Matrix &> H2 = boost::none) const override; | ||||
|   Vector evaluateError(const SOn& Q1, const SOn& Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override; | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
|   /// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp
 | ||||
|   void fillJacobians(const Matrix &M1, const Matrix &M2, | ||||
|                      boost::optional<Matrix &> H1, | ||||
|                      boost::optional<Matrix &> H2) const; | ||||
|                      OptionalMatrixType H1, | ||||
|                      OptionalMatrixType H2) const; | ||||
| }; | ||||
| 
 | ||||
| // Explicit instantiation for d=2 and d=3 in .cpp file:
 | ||||
|  |  | |||
|  | @ -45,6 +45,10 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> { | |||
|   Point3 measured_w_aZb_; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor2<Point3, Point3>::evaluateError; | ||||
| 
 | ||||
|   /// default constructor
 | ||||
|   TranslationFactor() {} | ||||
| 
 | ||||
|  | @ -65,8 +69,8 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> { | |||
|    */ | ||||
|   Vector evaluateError( | ||||
|       const Point3& Ta, const Point3& Tb, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, | ||||
|       OptionalMatrixType H2) const override { | ||||
|     const Point3 dir = Tb - Ta; | ||||
|     Matrix33 H_predicted_dir; | ||||
|     const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); | ||||
|  |  | |||
|  | @ -56,6 +56,9 @@ namespace gtsam { | |||
| 
 | ||||
|   public: | ||||
| 
 | ||||
|     // Provide access to the Matrix& version of evaluateError:
 | ||||
|     using Base::evaluateError; | ||||
| 
 | ||||
|     // shorthand for a smart pointer to a factor
 | ||||
|     typedef typename boost::shared_ptr<BetweenFactor> shared_ptr; | ||||
| 
 | ||||
|  | @ -105,13 +108,13 @@ namespace gtsam { | |||
|     /// @{
 | ||||
| 
 | ||||
|     /// evaluate error, returns vector of errors size of tangent space
 | ||||
|     Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 = | ||||
|       boost::none, boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|     Vector evaluateError(const T& p1, const T& p2, | ||||
| 			OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
 | ||||
|       // manifold equivalent of h(x)-z -> log(z,h(x))
 | ||||
| #ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR | ||||
|       typename traits<T>::ChartJacobian::Jacobian Hlocal; | ||||
|       Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); | ||||
|       Vector rval = traits<T>::Local(measured_, hx, OptionalNone, (H1 || H2) ? &Hlocal : 0); | ||||
|       if (H1) *H1 = Hlocal * (*H1); | ||||
|       if (H2) *H2 = Hlocal * (*H2); | ||||
|       return rval; | ||||
|  |  | |||
|  | @ -35,6 +35,9 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> { | |||
|   typedef NoiseModelFactorN<VALUE> Base; | ||||
|   typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   double threshold_; | ||||
|   bool isGreaterThan_; /// flag for greater/less than
 | ||||
| 
 | ||||
|  | @ -54,8 +57,8 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> { | |||
|    * Must have optional argument for derivative with 1xN matrix, where | ||||
|    * N = X::dim() | ||||
|    */ | ||||
|   virtual double value(const X& x, boost::optional<Matrix&> H = | ||||
|       boost::none) const = 0; | ||||
|   virtual double value(const X& x, OptionalMatrixType H = | ||||
|       OptionalNone) const = 0; | ||||
| 
 | ||||
|   /** active when constraint *NOT* met */ | ||||
|   bool active(const Values& c) const override { | ||||
|  | @ -64,10 +67,9 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> { | |||
|     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; | ||||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const X& x, boost::optional<Matrix&> H = | ||||
|       boost::none) const override { | ||||
|   Vector evaluateError(const X& x, OptionalMatrixType H) const override { | ||||
|     Matrix D; | ||||
|     double error = value(x, D) - threshold_; | ||||
|     double error = value(x, &D) - threshold_; | ||||
|     if (H) { | ||||
|       if (isGreaterThan_) *H = D; | ||||
|       else *H = -1.0 * D; | ||||
|  | @ -105,6 +107,9 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> { | |||
|   typedef NoiseModelFactorN<VALUE1, VALUE2> Base; | ||||
|   typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   double threshold_; | ||||
|   bool isGreaterThan_; /// flag for greater/less than
 | ||||
| 
 | ||||
|  | @ -123,8 +128,8 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> { | |||
|    * Must have optional argument for derivatives) | ||||
|    */ | ||||
|   virtual double value(const X1& x1, const X2& x2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const = 0; | ||||
|       OptionalMatrixType H1 = OptionalNone, | ||||
|       OptionalMatrixType H2 = OptionalNone) const = 0; | ||||
| 
 | ||||
|   /** active when constraint *NOT* met */ | ||||
|   bool active(const Values& c) const override { | ||||
|  | @ -134,10 +139,9 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> { | |||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const X1& x1, const X2& x2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     Matrix D1, D2; | ||||
|     double error = value(x1, x2, D1, D2) - threshold_; | ||||
|     double error = value(x1, x2, &D1, &D2) - threshold_; | ||||
|     if (H1) { | ||||
|       if (isGreaterThan_)  *H1 = D1; | ||||
|       else *H1 = -1.0 * D1; | ||||
|  |  | |||
|  | @ -43,16 +43,20 @@ bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1, | ||||
|     const Pose3& p2, boost::optional<Matrix&> Hp1, | ||||
|     boost::optional<Matrix&> Hp2) const { | ||||
|     const Pose3& p2, OptionalMatrixType Hp1, | ||||
|     OptionalMatrixType Hp2) const { | ||||
| 
 | ||||
|   // compute relative Pose3 between p1 and p2
 | ||||
|   Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); | ||||
| 
 | ||||
|   // convert to EssentialMatrix
 | ||||
|   Matrix D_hx_1P2; | ||||
|   EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, | ||||
|       (Hp1 || Hp2) ? boost::optional<Matrix&>(D_hx_1P2) : boost::none); | ||||
|   EssentialMatrix hx; | ||||
|   if (Hp1 || Hp2) { | ||||
|     hx = EssentialMatrix::FromPose3(_1P2_, D_hx_1P2); | ||||
|   } else { | ||||
|       hx = EssentialMatrix::FromPose3(_1P2_, OptionalNone); | ||||
|   } | ||||
| 
 | ||||
|   // Calculate derivatives if needed
 | ||||
|   if (Hp1) { | ||||
|  |  | |||
|  | @ -38,6 +38,9 @@ private: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   // shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr; | ||||
| 
 | ||||
|  | @ -79,8 +82,7 @@ public: | |||
| 
 | ||||
|   /** vector of errors */ | ||||
|   Vector evaluateError(const Pose3& p1, const Pose3& p2, | ||||
|       boost::optional<Matrix&> Hp1 = boost::none, //
 | ||||
|       boost::optional<Matrix&> Hp2 = boost::none) const override; | ||||
|       OptionalMatrixType Hp1, OptionalMatrixType Hp2) const override; | ||||
| 
 | ||||
|   /** return the measured */ | ||||
|   const EssentialMatrix& measured() const { | ||||
|  |  | |||
|  | @ -38,6 +38,10 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> { | |||
|   typedef EssentialMatrixFactor This; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Constructor | ||||
|    *  @param key Essential Matrix variable key | ||||
|  | @ -90,8 +94,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> { | |||
| 
 | ||||
|   /// vector of errors returns 1D vector
 | ||||
|   Vector evaluateError( | ||||
|       const EssentialMatrix& E, | ||||
|       boost::optional<Matrix&> H = boost::none) const override { | ||||
|       const EssentialMatrix& E, OptionalMatrixType H) const override { | ||||
|     Vector error(1); | ||||
|     error << E.error(vA_, vB_, H); | ||||
|     return error; | ||||
|  | @ -115,6 +118,10 @@ class EssentialMatrixFactor2 | |||
|   typedef EssentialMatrixFactor2 This; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Constructor | ||||
|    *  @param key1 Essential Matrix variable key | ||||
|  | @ -173,8 +180,7 @@ class EssentialMatrixFactor2 | |||
|    */ | ||||
|   Vector evaluateError( | ||||
|       const EssentialMatrix& E, const double& d, | ||||
|       boost::optional<Matrix&> DE = boost::none, | ||||
|       boost::optional<Matrix&> Dd = boost::none) const override { | ||||
|       OptionalMatrixType DE, OptionalMatrixType Dd) const override { | ||||
|     // We have point x,y in image 1
 | ||||
|     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
 | ||||
|     // We then convert to second camera by P2 = 1R2'*(P1-1T2)
 | ||||
|  | @ -235,6 +241,10 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { | |||
|   Rot3 cRb_;  ///< Rotation from body to camera frame
 | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Constructor | ||||
|    *  @param key1 Essential Matrix variable key | ||||
|  | @ -284,18 +294,19 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { | |||
|    */ | ||||
|   Vector evaluateError( | ||||
|       const EssentialMatrix& E, const double& d, | ||||
|       boost::optional<Matrix&> DE = boost::none, | ||||
|       boost::optional<Matrix&> Dd = boost::none) const override { | ||||
|       OptionalMatrixType DE, OptionalMatrixType Dd) const override { | ||||
|     if (!DE) { | ||||
|       // Convert E from body to camera frame
 | ||||
|       EssentialMatrix cameraE = cRb_ * E; | ||||
|       // Evaluate error
 | ||||
|       return Base::evaluateError(cameraE, d, boost::none, Dd); | ||||
|       return Base::evaluateError(cameraE, d, OptionalNone, Dd); | ||||
|     } else { | ||||
|       // Version with derivatives
 | ||||
|       Matrix D_e_cameraE, D_cameraE_E;  // 2*5, 5*5
 | ||||
|       EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); | ||||
|       Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd); | ||||
|       // Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2)
 | ||||
|       // does not have the matrix reference version of evaluateError
 | ||||
|       Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd); | ||||
|       *DE = D_e_cameraE * D_cameraE_E;  // (2*5) * (5*5)
 | ||||
|       return e; | ||||
|     } | ||||
|  | @ -332,6 +343,10 @@ class EssentialMatrixFactor4 | |||
|   typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|    // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Constructor | ||||
|    *  @param keyE Essential Matrix (from camera B to A) variable key | ||||
|  | @ -372,13 +387,12 @@ class EssentialMatrixFactor4 | |||
|    */ | ||||
|   Vector evaluateError( | ||||
|       const EssentialMatrix& E, const CALIBRATION& K, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     // converting from pixel coordinates to normalized coordinates cA and cB
 | ||||
|     JacobianCalibration cA_H_K;  // dcA/dK
 | ||||
|     JacobianCalibration cB_H_K;  // dcB/dK
 | ||||
|     Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none); | ||||
|     Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none); | ||||
|     Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone); | ||||
|     Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone); | ||||
| 
 | ||||
|     // convert to homogeneous coordinates
 | ||||
|     Vector3 vA = EssentialMatrix::Homogeneous(cA); | ||||
|  |  | |||
|  | @ -54,6 +54,10 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> { | |||
|   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
 | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<Rot>::evaluateError; | ||||
| 
 | ||||
|   EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||||
| 
 | ||||
|   /// Constructor
 | ||||
|  | @ -64,8 +68,7 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> { | |||
|   } | ||||
| 
 | ||||
|   /// Error is just Frobenius norm between Rot element and vectorized matrix M.
 | ||||
|   Vector evaluateError(const Rot& R, | ||||
|                        boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const Rot& R, OptionalMatrixType H) const override { | ||||
|     return R.vec(H) - vecM_;  // Jacobian is computed only when needed.
 | ||||
|   } | ||||
| }; | ||||
|  | @ -79,6 +82,10 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> { | |||
|   enum { Dim = Rot::VectorN2::RowsAtCompileTime }; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor2<Rot, Rot>::evaluateError; | ||||
| 
 | ||||
|   /// Constructor
 | ||||
|   FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) | ||||
|       : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1, | ||||
|  | @ -86,8 +93,7 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> { | |||
| 
 | ||||
|   /// Error is just Frobenius norm between rotation matrices.
 | ||||
|   Vector evaluateError(const Rot& R1, const Rot& R2, | ||||
|                        boost::optional<Matrix&> H1 = boost::none, | ||||
|                        boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|                        OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     Vector error = R2.vec(H2) - R1.vec(H1); | ||||
|     if (H1) *H1 = -*H1; | ||||
|     return error; | ||||
|  | @ -108,6 +114,10 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> { | |||
|   enum { Dim = Rot::VectorN2::RowsAtCompileTime }; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor2<Rot, Rot>::evaluateError; | ||||
| 
 | ||||
|   EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||||
| 
 | ||||
|   /// @name Constructor
 | ||||
|  | @ -150,8 +160,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> { | |||
| 
 | ||||
|   /// Error is Frobenius norm between R1*R12 and R2.
 | ||||
|   Vector evaluateError(const Rot& R1, const Rot& R2, | ||||
|                        boost::optional<Matrix&> H1 = boost::none, | ||||
|                        boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|                        OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     const Rot R2hat = R1.compose(R12_); | ||||
|     Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat; | ||||
|     Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); | ||||
|  |  | |||
|  | @ -76,6 +76,9 @@ public: | |||
|   typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
 | ||||
|   typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;///< typedef for the base class
 | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   // shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<This> shared_ptr; | ||||
| 
 | ||||
|  | @ -123,7 +126,7 @@ public: | |||
| 
 | ||||
|   /** h(x)-z */ | ||||
|   Vector evaluateError(const CAMERA& camera, const LANDMARK& point, | ||||
|       boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     try { | ||||
|       return camera.project2(point,H1,H2) - measured_; | ||||
|     } | ||||
|  | @ -258,10 +261,7 @@ public: | |||
| 
 | ||||
|   /** h(x)-z */ | ||||
|   Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none, | ||||
|       boost::optional<Matrix&> H3=boost::none) const override | ||||
|   { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override { | ||||
|     try { | ||||
|       Camera camera(pose3,calib); | ||||
|       return camera.project(point, H1, H2, H3) - measured_; | ||||
|  |  | |||
|  | @ -23,8 +23,8 @@ void OrientedPlane3Factor::print(const string& s, | |||
| 
 | ||||
| //***************************************************************************
 | ||||
| Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, | ||||
|     const OrientedPlane3& plane, boost::optional<Matrix&> H1, | ||||
|     boost::optional<Matrix&> H2) const { | ||||
|     const OrientedPlane3& plane, OptionalMatrixType H1, | ||||
|     OptionalMatrixType H2) const { | ||||
|   Matrix36 predicted_H_pose; | ||||
|   Matrix33 predicted_H_plane, error_H_predicted; | ||||
| 
 | ||||
|  | @ -64,7 +64,7 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, | |||
| 
 | ||||
| //***************************************************************************
 | ||||
| Vector OrientedPlane3DirectionPrior::evaluateError( | ||||
|     const OrientedPlane3& plane, boost::optional<Matrix&> H) const { | ||||
|     const OrientedPlane3& plane, OptionalMatrixType H) const { | ||||
|   Unit3 n_hat_p = measured_p_.normal(); | ||||
|   Unit3 n_hat_q = plane.normal(); | ||||
|   Matrix2 H_p; | ||||
|  |  | |||
|  | @ -21,6 +21,10 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente | |||
|   typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor2<Pose3, OrientedPlane3>::evaluateError; | ||||
| 
 | ||||
|   /// Constructor
 | ||||
|   OrientedPlane3Factor() { | ||||
|   } | ||||
|  | @ -44,8 +48,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente | |||
|   /// evaluateError
 | ||||
|   Vector evaluateError( | ||||
|       const Pose3& pose, const OrientedPlane3& plane, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override; | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override; | ||||
| }; | ||||
| 
 | ||||
| // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
 | ||||
|  | @ -55,6 +58,10 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien | |||
|   typedef NoiseModelFactorN<OrientedPlane3> Base; | ||||
| 
 | ||||
|  public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   typedef OrientedPlane3DirectionPrior This; | ||||
|   /// Constructor
 | ||||
|   OrientedPlane3DirectionPrior() { | ||||
|  | @ -72,8 +79,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien | |||
|   /// equals
 | ||||
|   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; | ||||
| 
 | ||||
|   Vector evaluateError(const OrientedPlane3& plane, | ||||
|       boost::optional<Matrix&> H = boost::none) const override; | ||||
|   Vector evaluateError(const OrientedPlane3& plane, OptionalMatrixType H) const override; | ||||
| }; | ||||
| 
 | ||||
| } // gtsam
 | ||||
|  |  | |||
|  | @ -25,6 +25,9 @@ public: | |||
|   typedef typename POSE::Translation Translation; | ||||
|   typedef typename POSE::Rotation Rotation; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   GTSAM_CONCEPT_POSE_TYPE(Pose) | ||||
|   GTSAM_CONCEPT_GROUP_TYPE(Pose) | ||||
|   GTSAM_CONCEPT_LIE_TYPE(Rotation) | ||||
|  | @ -75,7 +78,7 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /** h(x)-z */ | ||||
|   Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override { | ||||
|     const Rotation& newR = pose.rotation(); | ||||
|     if (H) { | ||||
|       *H = Matrix::Zero(rDim, xDim); | ||||
|  |  | |||
|  | @ -25,6 +25,9 @@ public: | |||
|   typedef POSE Pose; | ||||
|   typedef typename POSE::Translation Translation; | ||||
|   typedef typename POSE::Rotation Rotation; | ||||
|    | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   GTSAM_CONCEPT_POSE_TYPE(Pose) | ||||
|   GTSAM_CONCEPT_GROUP_TYPE(Pose) | ||||
|  | @ -59,7 +62,7 @@ public: | |||
|         gtsam::NonlinearFactor::shared_ptr(new This(*this))); } | ||||
| 
 | ||||
|   /** h(x)-z */ | ||||
|   Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override { | ||||
|     const Translation& newTrans = pose.translation(); | ||||
|     const Rotation& R = pose.rotation(); | ||||
|     const int tDim = traits<Translation>::GetDimension(newTrans); | ||||
|  |  | |||
|  | @ -54,6 +54,9 @@ namespace gtsam { | |||
|     /// shorthand for base class type
 | ||||
|     typedef NoiseModelFactorN<POSE, LANDMARK> Base; | ||||
| 
 | ||||
|     // Provide access to the Matrix& version of evaluateError:
 | ||||
|     using Base::evaluateError; | ||||
| 
 | ||||
|     /// shorthand for this class
 | ||||
|     typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This; | ||||
| 
 | ||||
|  | @ -133,7 +136,7 @@ namespace gtsam { | |||
| 
 | ||||
|     /// Evaluate error h(x)-z and optionally derivatives
 | ||||
|     Vector evaluateError(const Pose3& pose, const Point3& point, | ||||
|         boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|         OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|       try { | ||||
|         if(body_P_sensor_) { | ||||
|           if(H1) { | ||||
|  |  | |||
|  | @ -29,8 +29,8 @@ namespace gtsam { | |||
| template<class T, class P> | ||||
| P transform_point( | ||||
|     const T& trans, const P& global, | ||||
|     boost::optional<Matrix&> Dtrans, | ||||
|     boost::optional<Matrix&> Dglobal) { | ||||
|     OptionalMatrixType Dtrans, | ||||
|     OptionalMatrixType Dglobal) { | ||||
|   return trans.transformFrom(global, Dtrans, Dglobal); | ||||
| } | ||||
| 
 | ||||
|  | @ -63,6 +63,9 @@ public: | |||
|   typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base; | ||||
|   typedef ReferenceFrameFactor<POINT, TRANSFORM> This; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   typedef POINT Point; | ||||
|   typedef TRANSFORM Transform; | ||||
| 
 | ||||
|  | @ -95,12 +98,12 @@ public: | |||
| 
 | ||||
|   /** Combined cost and derivative function using boost::optional */ | ||||
|   Vector evaluateError(const Point& global, const Transform& trans, const Point& local, | ||||
|         boost::optional<Matrix&> Dforeign = boost::none, | ||||
|         boost::optional<Matrix&> Dtrans = boost::none, | ||||
|         boost::optional<Matrix&> Dlocal = boost::none) const override { | ||||
|         OptionalMatrixType Dforeign, OptionalMatrixType Dtrans, | ||||
|         OptionalMatrixType Dlocal) const override { | ||||
|     Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign); | ||||
|     if (Dlocal) | ||||
|     if (Dlocal) { | ||||
|       *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension); | ||||
|     } | ||||
|     return traits<Point>::Local(local,newlocal); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -29,6 +29,9 @@ class RotateFactor: public NoiseModelFactorN<Rot3> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// Constructor
 | ||||
|   RotateFactor(Key key, const Rot3& P, const Rot3& Z, | ||||
|       const SharedNoiseModel& model) : | ||||
|  | @ -50,8 +53,7 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// vector of errors returns 2D vector
 | ||||
|   Vector evaluateError(const Rot3& R, | ||||
|       boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const Rot3& R, OptionalMatrixType H) const override { | ||||
|     // predict p_ as q = R*z_, derivative H will be filled if not none
 | ||||
|     Point3 q = R.rotate(z_,H); | ||||
|     // error is just difference, and note derivative of that wrpt q is I3
 | ||||
|  | @ -73,6 +75,9 @@ class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// Constructor
 | ||||
|   RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z, | ||||
|       const SharedNoiseModel& model) : | ||||
|  | @ -102,7 +107,7 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// vector of errors returns 2D vector
 | ||||
|   Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override { | ||||
|   Vector evaluateError(const Rot3& iRc, OptionalMatrixType H) const override { | ||||
|     Unit3 i_q = iRc * c_z_; | ||||
|     Vector error = i_p_.error(i_q, H); | ||||
|     if (H) { | ||||
|  |  | |||
|  | @ -198,13 +198,16 @@ protected: | |||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
 | ||||
|   /// derivatives. This is the error before the noise model is applied.
 | ||||
|   /** Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
 | ||||
|   * 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,  //
 | ||||
|       boost::optional<Matrix&> E = boost::none) const { | ||||
|       typename Cameras::FBlocks* Fs = nullptr,  //
 | ||||
|       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, | ||||
|       boost::optional<Matrix&> E = boost::none) const {} | ||||
|       typename Cameras::FBlocks* Fs = nullptr, | ||||
|       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); | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  |  | |||
|  | @ -337,8 +337,9 @@ protected: | |||
|    */ | ||||
|   bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { | ||||
|     bool nonDegenerate = triangulateForLinearize(cameras); | ||||
|     if (nonDegenerate) | ||||
|       cameras.project2(*result_, boost::none, E); | ||||
|     if (nonDegenerate) { | ||||
|       cameras.project2(*result_, nullptr, &E); | ||||
|     } | ||||
|     return nonDegenerate; | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -48,6 +48,9 @@ public: | |||
|   typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  ///< typedef for shared pointer to this object
 | ||||
|   typedef POSE CamPose;                                       ///< typedef for Pose Lie Value type
 | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Default constructor | ||||
|    */ | ||||
|  | @ -120,7 +123,7 @@ public: | |||
| 
 | ||||
|   /** h(x)-z */ | ||||
|   Vector evaluateError(const Pose3& pose, const Point3& point, | ||||
|       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     try { | ||||
|       if(body_P_sensor_) { | ||||
|         if(H1) { | ||||
|  |  | |||
|  | @ -62,6 +62,9 @@ public: | |||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   using shared_ptr = boost::shared_ptr<This>; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<Point3>::evaluateError; | ||||
| 
 | ||||
|   /// Default constructor
 | ||||
|   TriangulationFactor() : | ||||
|       throwCheirality_(false), verboseCheirality_(false) { | ||||
|  | @ -119,10 +122,9 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// Evaluate error h(x)-z and optionally derivatives
 | ||||
|   Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 = | ||||
|       boost::none) const override { | ||||
|   Vector evaluateError(const Point3& point, OptionalMatrixType H2) const override { | ||||
|     try { | ||||
|       return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2)); | ||||
|       return traits<Measurement>::Local(measured_, camera_.project2(point, OptionalNone, H2)); | ||||
|     } catch (CheiralityException& e) { | ||||
|       if (H2) | ||||
|         *H2 = Matrix::Zero(traits<Measurement>::dimension, 3); | ||||
|  |  | |||
|  | @ -36,16 +36,13 @@ TEST(BetweenFactor, Rot3) { | |||
|   EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail
 | ||||
| 
 | ||||
|   Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>( | ||||
|       std::function<Vector(const Rot3&, const Rot3&)>(std::bind( | ||||
|           &BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1, | ||||
|           std::placeholders::_2, boost::none, boost::none)), | ||||
|         [&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);}, | ||||
|       R1, R2, 1e-5); | ||||
|   EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); | ||||
| 
 | ||||
|   Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>( | ||||
|       std::function<Vector(const Rot3&, const Rot3&)>(std::bind( | ||||
|           &BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none, | ||||
|           boost::none)), R1, R2, 1e-5); | ||||
|         [&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);}, | ||||
|         R1, R2, 1e-5); | ||||
|   EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -53,12 +53,11 @@ TEST( EssentialMatrixConstraint, test ) { | |||
| 
 | ||||
|   // Calculate numerical derivatives
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector5, Pose3>( | ||||
|       std::bind(&EssentialMatrixConstraint::evaluateError, &factor, | ||||
|                 std::placeholders::_1, pose2, boost::none, boost::none), | ||||
|       pose1); | ||||
| 		[&factor, &pose2](const Pose3& p1) {return factor.evaluateError(p1, pose2);}, | ||||
| 		pose1); | ||||
| 
 | ||||
|   Matrix expectedH2 = numericalDerivative11<Vector5, Pose3>( | ||||
|       std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, | ||||
|                 std::placeholders::_1, boost::none, boost::none), | ||||
| 		[&factor, &pose1](const Pose3& p2) {return factor.evaluateError(pose1, p2);}, | ||||
|       pose2); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative
 | ||||
|  |  | |||
|  | @ -140,9 +140,9 @@ TEST( OrientedPlane3Factor, Derivatives ) { | |||
|   OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); | ||||
| 
 | ||||
|   // Calculate numerical derivatives
 | ||||
|   std::function<Vector(const Pose3 &, const OrientedPlane3 &)> f = std::bind( | ||||
|       &OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1, | ||||
|       std::placeholders::_2, boost::none, boost::none); | ||||
|   auto f = [&factor](const Pose3& p, const OrientedPlane3& o) { | ||||
|     return factor.evaluateError(p, o); | ||||
|   }; | ||||
|   Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin); | ||||
|   Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin); | ||||
| 
 | ||||
|  | @ -180,16 +180,13 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { | |||
| 
 | ||||
|   // Calculate numerical derivatives
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>( | ||||
|       std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, | ||||
|           boost::none), T1); | ||||
| 		  [&factor](const OrientedPlane3& o) {return factor.evaluateError(o);}, T1); | ||||
| 
 | ||||
|   Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>( | ||||
|       std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, | ||||
|           boost::none), T2); | ||||
|       [&factor](const OrientedPlane3& o) {return factor.evaluateError(o);}, T2); | ||||
| 
 | ||||
|   Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>( | ||||
|       std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, | ||||
|           boost::none), T3); | ||||
|       [&factor](const OrientedPlane3& o) { return factor.evaluateError(o); }, T3); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative
 | ||||
|   Matrix actualH1, actualH2, actualH3; | ||||
|  |  | |||
|  | @ -69,14 +69,14 @@ TEST (RotateFactor, test) { | |||
|   Matrix actual, expected; | ||||
|   // Use numerical derivatives to calculate the expected Jacobian
 | ||||
|   { | ||||
|     expected = numericalDerivative11<Vector3,Rot3>( | ||||
|         std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc); | ||||
|     expected = numericalDerivative11<Vector3, Rot3>( | ||||
| 			[&f](const Rot3& r) { return f.evaluateError(r); }, iRc); | ||||
|     f.evaluateError(iRc, actual); | ||||
|     EXPECT(assert_equal(expected, actual, 1e-9)); | ||||
|   } | ||||
|   { | ||||
|     expected = numericalDerivative11<Vector3,Rot3>( | ||||
|         std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R); | ||||
|     expected = numericalDerivative11<Vector3, Rot3>( | ||||
| 			[&f](const Rot3& r) { return f.evaluateError(r); }, R); | ||||
|     f.evaluateError(R, actual); | ||||
|     EXPECT(assert_equal(expected, actual, 1e-9)); | ||||
|   } | ||||
|  | @ -141,15 +141,13 @@ TEST (RotateDirectionsFactor, test) { | |||
|   // Use numerical derivatives to calculate the expected Jacobian
 | ||||
|   { | ||||
|     expected = numericalDerivative11<Vector,Rot3>( | ||||
|         std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, | ||||
|             boost::none), iRc); | ||||
| 			[&f](const Rot3& r) {return f.evaluateError(r);}, iRc); | ||||
|     f.evaluateError(iRc, actual); | ||||
|     EXPECT(assert_equal(expected, actual, 1e-9)); | ||||
|   } | ||||
|   { | ||||
|     expected = numericalDerivative11<Vector,Rot3>( | ||||
|         std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, | ||||
|             boost::none), R); | ||||
| 			[&f](const Rot3& r) {return f.evaluateError(r);}, R); | ||||
|     f.evaluateError(R, actual); | ||||
|     EXPECT(assert_equal(expected, actual, 1e-9)); | ||||
|   } | ||||
|  |  | |||
|  | @ -59,7 +59,7 @@ TEST( triangulation, TriangulationFactor ) { | |||
|   factor.evaluateError(landmark, HActual); | ||||
| 
 | ||||
|   Matrix HExpected = numericalDerivative11<Vector,Point3>( | ||||
|       std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); | ||||
| 		  [&factor](const Point3& l) { return factor.evaluateError(l);}, landmark); | ||||
| 
 | ||||
|   // Verify the Jacobians are correct
 | ||||
|   CHECK(assert_equal(HExpected, HActual, 1e-3)); | ||||
|  | @ -82,8 +82,8 @@ TEST( triangulation, TriangulationFactorStereo ) { | |||
|   Matrix HActual; | ||||
|   factor.evaluateError(landmark, HActual); | ||||
| 
 | ||||
|   Matrix HExpected = numericalDerivative11<Vector,Point3>( | ||||
|       std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); | ||||
|   Matrix HExpected = numericalDerivative11<Vector, Point3>( | ||||
| 		  [&factor](const Point3& l) { return factor.evaluateError(l);}, landmark); | ||||
| 
 | ||||
|   // Verify the Jacobians are correct
 | ||||
|   CHECK(assert_equal(HExpected, HActual, 1e-3)); | ||||
|  |  | |||
|  | @ -35,6 +35,9 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /** Standard constructor */ | ||||
|   FullIMUFactor(const Vector3& accel, const Vector3& gyro, | ||||
|       double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) | ||||
|  | @ -84,8 +87,7 @@ public: | |||
|    *  z - h(x1,x2) | ||||
|    */ | ||||
|   Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     Vector9 z; | ||||
|     z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
 | ||||
|     z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
 | ||||
|  | @ -99,8 +101,7 @@ public: | |||
| 
 | ||||
|   /** dummy version that fails for non-dynamic poses */ | ||||
|   virtual Vector evaluateError(const Pose3& x1, const Pose3& x2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const { | ||||
|     assert(false); | ||||
|     return Vector6::Zero(); | ||||
|   } | ||||
|  |  | |||
|  | @ -33,6 +33,9 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /** Standard constructor */ | ||||
|   IMUFactor(const Vector3& accel, const Vector3& gyro, | ||||
|       double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) | ||||
|  | @ -77,8 +80,7 @@ public: | |||
|    *  z - h(x1,x2) | ||||
|    */ | ||||
|   Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     const Vector6 meas = z(); | ||||
|     if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>( | ||||
|         std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); | ||||
|  | @ -89,8 +91,7 @@ public: | |||
| 
 | ||||
|   /** dummy version that fails for non-dynamic poses */ | ||||
|   virtual Vector evaluateError(const Pose3& x1, const Pose3& x2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const { | ||||
|     assert(false); // no corresponding factor here
 | ||||
|     return Vector6::Zero(); | ||||
|   } | ||||
|  |  | |||
|  | @ -33,6 +33,9 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   typedef boost::shared_ptr<PendulumFactor1> shared_ptr; | ||||
| 
 | ||||
|   ///Constructor.  k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
 | ||||
|  | @ -46,9 +49,8 @@ public: | |||
| 
 | ||||
|   /** q_k + h*v - q_k1 = 0, with optional derivatives */ | ||||
|   Vector evaluateError(const double& qk1, const double& qk, const double& v, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const override { | ||||
|     const size_t p = 1; | ||||
|     if (H1) *H1 = -Matrix::Identity(p,p); | ||||
|     if (H2) *H2 = Matrix::Identity(p,p); | ||||
|  | @ -81,6 +83,9 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   typedef boost::shared_ptr<PendulumFactor2 > shared_ptr; | ||||
| 
 | ||||
|   ///Constructor.  vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
 | ||||
|  | @ -94,9 +99,8 @@ public: | |||
| 
 | ||||
|   /**  v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ | ||||
|   Vector evaluateError(const double & vk1, const double & vk, const double & q, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const override { | ||||
|     const size_t p = 1; | ||||
|     if (H1) *H1 = -Matrix::Identity(p,p); | ||||
|     if (H2) *H2 = Matrix::Identity(p,p); | ||||
|  | @ -130,6 +134,9 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   typedef boost::shared_ptr<PendulumFactorPk > shared_ptr; | ||||
| 
 | ||||
|   ///Constructor
 | ||||
|  | @ -145,9 +152,8 @@ public: | |||
| 
 | ||||
|   /**  1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ | ||||
|   Vector evaluateError(const double & pk, const double & qk, const double & qk1, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const override { | ||||
|     const size_t p = 1; | ||||
| 
 | ||||
|     double qmid = (1-alpha_)*qk + alpha_*qk1; | ||||
|  | @ -186,6 +192,9 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr; | ||||
| 
 | ||||
|   ///Constructor
 | ||||
|  | @ -201,9 +210,8 @@ public: | |||
| 
 | ||||
|   /**  1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ | ||||
|   Vector evaluateError(const double & pk1, const double & qk, const double & qk1, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const override { | ||||
|     const size_t p = 1; | ||||
| 
 | ||||
|     double qmid = (1-alpha_)*qk + alpha_*qk1; | ||||
|  |  | |||
|  | @ -29,6 +29,10 @@ class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6>  { | |||
|   double h_;  // time step
 | ||||
|   typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base; | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : | ||||
|     Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, | ||||
|         xiKey), h_(h) { | ||||
|  | @ -42,9 +46,8 @@ public: | |||
| 
 | ||||
|   /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0 \f$, with optional derivatives */ | ||||
|   Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const override { | ||||
| 
 | ||||
|     Matrix6 D_exphxi_xi; | ||||
|     Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0); | ||||
|  | @ -89,6 +92,10 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector | |||
|   typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, | ||||
|       double h, const Matrix& Inertia, const Vector& Fu, double m, | ||||
|       double mu = 1000.0) : | ||||
|  | @ -108,9 +115,8 @@ public: | |||
|    *       pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1 | ||||
|    * */ | ||||
|   Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const override { | ||||
| 
 | ||||
|     Vector muk = Inertia_*xik; | ||||
|     Vector muk_1 = Inertia_*xik_1; | ||||
|  | @ -161,9 +167,8 @@ public: | |||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const { | ||||
|     if (H1) { | ||||
|       (*H1) = numericalDerivative31( | ||||
|           std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>( | ||||
|  |  | |||
|  | @ -34,7 +34,10 @@ typedef enum { | |||
|  */ | ||||
| class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> { | ||||
| public: | ||||
|   typedef gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> Base; | ||||
|   typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
| protected: | ||||
| 
 | ||||
|  | @ -83,8 +86,7 @@ public: | |||
|    * Calculates the error for trapezoidal model given | ||||
|    */ | ||||
|   gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, | ||||
|       boost::optional<gtsam::Matrix&> H1=boost::none, | ||||
|       boost::optional<gtsam::Matrix&> H2=boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
|     if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>( | ||||
|         std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, | ||||
|             std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); | ||||
|  |  | |||
|  | @ -23,6 +23,9 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   typedef boost::shared_ptr<VelocityConstraint3 > shared_ptr; | ||||
| 
 | ||||
|   ///TODO: comment
 | ||||
|  | @ -37,9 +40,8 @@ public: | |||
| 
 | ||||
|   /** x1 + v*dt - x2 = 0, with optional derivatives */ | ||||
|   Vector evaluateError(const double& x1, const double& x2, const double& v, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|       OptionalMatrixType H3) const override { | ||||
|     const size_t p = 1; | ||||
|     if (H1) *H1 = Matrix::Identity(p,p); | ||||
|     if (H2) *H2 = -Matrix::Identity(p,p); | ||||
|  |  | |||
|  | @ -7,6 +7,8 @@ | |||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam_unstable/dynamics/SimpleHelicopter.h> | ||||
| #include "gtsam/base/Vector.h" | ||||
| #include "gtsam/geometry/Pose3.h" | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| using namespace std::placeholders; | ||||
|  | @ -56,29 +58,16 @@ TEST( Reconstruction, evaluateError) { | |||
|   EXPECT( | ||||
|       assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); | ||||
| 
 | ||||
|   Matrix numericalH1 = numericalDerivative31( | ||||
|       std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>( | ||||
|           std::bind(&Reconstruction::evaluateError, constraint, | ||||
|                     std::placeholders::_1, std::placeholders::_2, | ||||
|                     std::placeholders::_3, boost::none, boost::none, | ||||
|                     boost::none)), | ||||
|       g2, g1, V1_g1, 1e-5); | ||||
|   std::function<Vector(const Pose3&, const Pose3&, const Vector6&)> f =  | ||||
|     [&constraint](const Pose3& a1, const Pose3& a2, const Vector6& a3) { | ||||
|     return constraint.evaluateError(a1, a2, a3); | ||||
|   }; | ||||
| 
 | ||||
|   Matrix numericalH2 = numericalDerivative32( | ||||
|       std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>( | ||||
|           std::bind(&Reconstruction::evaluateError, constraint, | ||||
|                     std::placeholders::_1, std::placeholders::_2, | ||||
|                     std::placeholders::_3, boost::none, boost::none, | ||||
|                     boost::none)), | ||||
|       g2, g1, V1_g1, 1e-5); | ||||
|   Matrix numericalH1 = numericalDerivative31(f, g2, g1, V1_g1, 1e-5); | ||||
| 
 | ||||
|   Matrix numericalH3 = numericalDerivative33( | ||||
|       std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>( | ||||
|           std::bind(&Reconstruction::evaluateError, constraint, | ||||
|                     std::placeholders::_1, std::placeholders::_2, | ||||
|                     std::placeholders::_3, boost::none, boost::none, | ||||
|                     boost::none)), | ||||
|       g2, g1, V1_g1, 1e-5); | ||||
|   Matrix numericalH2 = numericalDerivative32(f, g2, g1, V1_g1, 1e-5); | ||||
| 
 | ||||
|   Matrix numericalH3 = numericalDerivative33(f, g2, g1, V1_g1, 1e-5); | ||||
| 
 | ||||
|   EXPECT(assert_equal(numericalH1,H1,1e-5)); | ||||
|   EXPECT(assert_equal(numericalH2,H2,1e-5)); | ||||
|  | @ -118,26 +107,16 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { | |||
|   Matrix H1, H2, H3; | ||||
|   EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); | ||||
| 
 | ||||
|   Matrix numericalH1 = numericalDerivative31( | ||||
|       std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>( | ||||
|           std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) | ||||
|           ), | ||||
|           expectedv2, V1_g1, g2, 1e-5 | ||||
|       ); | ||||
|   std::function<Vector(const Vector6&, const Vector6&, const Pose3&)> f = | ||||
|       [&constraint](const Vector6& a1, const Vector6& a2, const Pose3& a3) { | ||||
|         return constraint.evaluateError(a1, a2, a3); | ||||
|       }; | ||||
| 
 | ||||
|   Matrix numericalH2 = numericalDerivative32( | ||||
|       std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>( | ||||
|           std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) | ||||
|           ), | ||||
|           expectedv2, V1_g1, g2, 1e-5 | ||||
|       ); | ||||
|   Matrix numericalH1 = numericalDerivative31(f, expectedv2, V1_g1, g2, 1e-5); | ||||
| 
 | ||||
|   Matrix numericalH3 = numericalDerivative33( | ||||
|       std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>( | ||||
|           std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) | ||||
|           ), | ||||
|           expectedv2, V1_g1, g2, 1e-5 | ||||
|       ); | ||||
|   Matrix numericalH2 = numericalDerivative32(f, expectedv2, V1_g1, g2, 1e-5); | ||||
| 
 | ||||
|   Matrix numericalH3 = numericalDerivative33(f, expectedv2, V1_g1, g2, 1e-5); | ||||
| 
 | ||||
|   EXPECT(assert_equal(numericalH1,H1,1e-5)); | ||||
|   EXPECT(assert_equal(numericalH2,H2,1e-5)); | ||||
|  |  | |||
|  | @ -19,6 +19,7 @@ | |||
| #include <gtsam/geometry/Point2.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -83,9 +84,9 @@ public: | |||
|    */ | ||||
|   inline gtsam::Point2 project(const Vector5& pw, | ||||
|       double rho, | ||||
|       boost::optional<gtsam::Matrix&> H1 = boost::none, | ||||
|       boost::optional<gtsam::Matrix&> H2 = boost::none, | ||||
|       boost::optional<gtsam::Matrix&> H3 = boost::none) const { | ||||
|       OptionalJacobian<2,6> H1 = {}, | ||||
|       OptionalJacobian<2,5> H2 = {}, | ||||
|       OptionalJacobian<2,1> H3 = {}) const { | ||||
| 
 | ||||
|     gtsam::Point3 ray_base(pw.segment(0,3)); | ||||
|     double theta = pw(3), phi = pw(4); | ||||
|  |  | |||
|  | @ -5,7 +5,10 @@ | |||
|  * @author Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #include <boost/mpl/assert.hpp> | ||||
| #include <iostream> | ||||
| #include "gtsam/base/OptionalJacobian.h" | ||||
| #include "gtsam/base/Vector.h" | ||||
| 
 | ||||
| #include <gtsam_unstable/geometry/Pose3Upright.h> | ||||
| 
 | ||||
|  | @ -78,27 +81,34 @@ Pose3 Pose3Upright::pose() const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const { | ||||
|   Pose3Upright result(T_.inverse(H1), -z_); | ||||
|   if (H1) { | ||||
|     Matrix H1_ = -I_4x4; | ||||
|     H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); | ||||
|     H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); | ||||
|     *H1 = H1_; | ||||
| Pose3Upright Pose3Upright::inverse(OptionalJacobian<4, 4> H1) const { | ||||
|   if (!H1) { | ||||
|     return Pose3Upright(T_.inverse(), -z_); | ||||
|   } | ||||
|   OptionalJacobian<3, 3>::Jacobian H3x3; | ||||
|   // TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
 | ||||
|   // Eigen::Ref<Eigen::Matrix<double, 3, 3>> H3x3 = H1->topLeftCorner(3,3);
 | ||||
|   Pose3Upright result(T_.inverse(H3x3), -z_); | ||||
|   Matrix H1_ = -I_4x4; | ||||
|   H1_.topLeftCorner(2, 2) = H3x3.topLeftCorner(2, 2); | ||||
|   H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1); | ||||
|   *H1 = H1_; | ||||
|   return result; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { | ||||
|     OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const { | ||||
|   if (!H1 && !H2) | ||||
|     return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_); | ||||
|   Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_); | ||||
| 
 | ||||
|   // TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
 | ||||
|   OptionalJacobian<3, 3>::Jacobian H3x3; | ||||
|   Pose3Upright result(T_.compose(p2.T_, H3x3), z_ + p2.z_); | ||||
|   if (H1) { | ||||
|     Matrix H1_ = I_4x4; | ||||
|     H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); | ||||
|     H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); | ||||
|     H1_.topLeftCorner(2,2) = H3x3.topLeftCorner(2,2); | ||||
|     H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1); | ||||
|     *H1 = H1_; | ||||
|   } | ||||
|   if (H2) *H2 = I_4x4; | ||||
|  | @ -107,14 +117,17 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Pose3Upright Pose3Upright::between(const Pose3Upright& p2, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { | ||||
|     OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const { | ||||
|   if (!H1 && !H2) | ||||
|     return Pose3Upright(T_.between(p2.T_), p2.z_ - z_); | ||||
|   Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_); | ||||
| 
 | ||||
|   // TODO(kartikarcot): Could not use reference to a view into H1 and H2 to reuse memory
 | ||||
|   OptionalJacobian<3, 3>::Jacobian H3x3_1, H3x3_2; | ||||
|   Pose3Upright result(T_.between(p2.T_, H3x3_1, H3x3_2), p2.z_ - z_); | ||||
|   if (H1) { | ||||
|     Matrix H1_ = -I_4x4; | ||||
|     H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); | ||||
|     H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); | ||||
|     H1_.topLeftCorner(2,2) = H3x3_1.topLeftCorner(2,2); | ||||
|     H1_.topRightCorner(2, 1) = H3x3_1.topRightCorner(2, 1); | ||||
|     *H1 = H1_; | ||||
|   } | ||||
|   if (H2) *H2 = I_4x4; | ||||
|  |  | |||
|  | @ -98,12 +98,12 @@ public: | |||
|   static Pose3Upright Identity() { return Pose3Upright(); } | ||||
| 
 | ||||
|   /// inverse transformation with derivatives
 | ||||
|   Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const; | ||||
|   Pose3Upright inverse(OptionalJacobian<4,4> H1=boost::none) const; | ||||
| 
 | ||||
|   ///compose this transformation onto another (first *this and then p2)
 | ||||
|   Pose3Upright compose(const Pose3Upright& p2, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none) const; | ||||
|       OptionalJacobian<4,4> H1=boost::none, | ||||
|       OptionalJacobian<4,4> H2=boost::none) const; | ||||
| 
 | ||||
|   /// compose syntactic sugar
 | ||||
|   inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); } | ||||
|  | @ -113,8 +113,8 @@ public: | |||
|    * as well as optionally the derivatives | ||||
|    */ | ||||
|   Pose3Upright between(const Pose3Upright& p2, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none) const; | ||||
|       OptionalJacobian<4,4> H1=boost::none, | ||||
|       OptionalJacobian<4,4> H2=boost::none) const; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Lie Group
 | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ bool SimWall2D::equals(const SimWall2D& other, double tol) const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) const { | ||||
| bool SimWall2D::intersects(const SimWall2D& B, Point2* pt) const { | ||||
|   const bool debug = false; | ||||
| 
 | ||||
|   const SimWall2D& A = *this; | ||||
|  |  | |||
|  | @ -51,7 +51,15 @@ namespace gtsam { | |||
|      * returns true if they intersect, with the intersection | ||||
|      * point in the optional second argument | ||||
|      */ | ||||
|     bool intersects(const SimWall2D& wall, boost::optional<Point2&> pt=boost::none) const; | ||||
|     bool intersects(const SimWall2D& wall, Point2* pt = nullptr) const; | ||||
|      | ||||
|     /**
 | ||||
|      * An overload of intersects that takes an l-value reference to a Point2 | ||||
|      * instead of a pointer. | ||||
|      */ | ||||
|     bool intersects(const SimWall2D& wall, Point2& pt) const { | ||||
|       return intersects(wall, &pt); | ||||
|     } | ||||
| 
 | ||||
|     /**
 | ||||
|      * norm is a 2D point representing the norm of the wall | ||||
|  |  | |||
|  | @ -94,7 +94,7 @@ TEST( InvDepthFactor, Dproject_pose) | |||
|   Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); | ||||
|   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); | ||||
|   Matrix actual; | ||||
|   inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); | ||||
|   inv_camera.project(landmark, inv_depth, actual, {}, {}); | ||||
|   EXPECT(assert_equal(expected,actual,1e-6)); | ||||
| } | ||||
| 
 | ||||
|  | @ -106,7 +106,7 @@ TEST( InvDepthFactor, Dproject_landmark) | |||
|   Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); | ||||
|   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); | ||||
|   Matrix actual; | ||||
|   inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); | ||||
|   inv_camera.project(landmark, inv_depth, {}, actual, {}); | ||||
|   EXPECT(assert_equal(expected,actual,1e-7)); | ||||
| } | ||||
| 
 | ||||
|  | @ -118,7 +118,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) | |||
|   Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); | ||||
|   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); | ||||
|   Matrix actual; | ||||
|   inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); | ||||
|   inv_camera.project(landmark, inv_depth, {}, {}, actual); | ||||
|   EXPECT(assert_equal(expected,actual,1e-7)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -145,7 +145,7 @@ public: | |||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Vector whitenedError(const Values& x, | ||||
|       boost::optional<std::vector<Matrix>&> H = boost::none) const { | ||||
|       OptionalMatrixVecType H = nullptr) const { | ||||
| 
 | ||||
|     bool debug = true; | ||||
| 
 | ||||
|  | @ -228,6 +228,12 @@ public: | |||
|     return err_wh_eq; | ||||
|   } | ||||
| 
 | ||||
|   // A function overload that takes a vector of matrices and passes it to the
 | ||||
|   // function above which uses a pointer to a vector instead.
 | ||||
|   Vector whitenedError(const Values& x, std::vector<Matrix>& H) const { | ||||
| 	  return whitenedError(x, &H); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Vector calcIndicatorProb(const Values& x) const { | ||||
| 
 | ||||
|  |  | |||
|  | @ -38,6 +38,9 @@ namespace gtsam { | |||
| 
 | ||||
|   public: | ||||
| 
 | ||||
|     // Provide access to the Matrix& version of evaluateError:
 | ||||
|     using Base::evaluateError; | ||||
| 
 | ||||
|     // shorthand for a smart pointer to a factor
 | ||||
|     typedef boost::shared_ptr<BiasedGPSFactor> shared_ptr; | ||||
| 
 | ||||
|  | @ -73,8 +76,7 @@ namespace gtsam { | |||
| 
 | ||||
|     /** vector of errors */ | ||||
|     Vector evaluateError(const Pose3& pose, const Point3& bias, | ||||
|         boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = | ||||
|             boost::none) const override { | ||||
|         OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
| 
 | ||||
|       if (H1 || H2){ | ||||
|         H1->resize(3,6); // jacobian wrt pose
 | ||||
|  |  | |||
|  | @ -110,6 +110,9 @@ private: | |||
|   boost::optional<POSE> body_P_sensor_;   // The pose of the sensor in the body frame
 | ||||
| 
 | ||||
| public: | ||||
| 	 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   // shorthand for a smart pointer to a factor
 | ||||
|   typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr; | ||||
|  | @ -299,11 +302,8 @@ public: | |||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, | ||||
|       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) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, | ||||
|       OptionalMatrixType H5) const override { | ||||
| 
 | ||||
|     // TODO: Write analytical derivative calculations
 | ||||
|     // Jacobian w.r.t. Pose1
 | ||||
|  |  | |||
|  | @ -109,6 +109,9 @@ private: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   // shorthand for a smart pointer to a factor
 | ||||
|   typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr; | ||||
| 
 | ||||
|  | @ -270,10 +273,8 @@ public: | |||
|   } | ||||
| 
 | ||||
|   Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none, | ||||
|       boost::optional<Matrix&> H4 = boost::none) const { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, | ||||
|       OptionalMatrixType H4) const { | ||||
| 
 | ||||
|     // TODO: Write analytical derivative calculations
 | ||||
|     // Jacobian w.r.t. Pose1
 | ||||
|  |  | |||
|  | @ -54,6 +54,9 @@ private: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   // shorthand for a smart pointer to a factor
 | ||||
|   typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr; | ||||
| 
 | ||||
|  | @ -88,8 +91,7 @@ public: | |||
| 
 | ||||
|   /** vector of errors */ | ||||
|   Vector evaluateError(const VALUE& p1, const VALUE& p2, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
| 
 | ||||
|     Vector v1( traits<VALUE>::Logmap(p1) ); | ||||
|     Vector v2( traits<VALUE>::Logmap(p2) ); | ||||
|  |  | |||
|  | @ -96,6 +96,9 @@ private: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   // shorthand for a smart pointer to a factor
 | ||||
|   typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr; | ||||
| 
 | ||||
|  | @ -226,11 +229,8 @@ public: | |||
| 
 | ||||
|   /** implement functions needed to derive from Factor */ | ||||
|   Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, | ||||
|       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) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, | ||||
|       OptionalMatrixType H5) const override { | ||||
| 
 | ||||
|     // TODO: Write analytical derivative calculations
 | ||||
|     // Jacobian w.r.t. Pose1
 | ||||
|  |  | |||
|  | @ -34,7 +34,10 @@ protected: | |||
| public: | ||||
| 
 | ||||
|   /// shorthand for base class type
 | ||||
|   typedef NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> Base; | ||||
|   typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// shorthand for this class
 | ||||
|   typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This; | ||||
|  | @ -83,9 +86,7 @@ public: | |||
| 
 | ||||
|   /// Evaluate error h(x)-z and optionally derivatives
 | ||||
|   Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none, | ||||
|       boost::optional<Matrix&> H3=boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override { | ||||
|     try { | ||||
|       InvDepthCamera3<Cal3_S2> camera(pose, K_); | ||||
|       return camera.project(point, invDepth, H1, H2, H3) - measured_; | ||||
|  |  | |||
|  | @ -34,7 +34,10 @@ protected: | |||
| public: | ||||
| 
 | ||||
|   /// shorthand for base class type
 | ||||
|   typedef NoiseModelFactorN<Pose3, Vector6> Base; | ||||
|   typedef NoiseModelFactor2<Pose3, Vector6> Base; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// shorthand for this class
 | ||||
|   typedef InvDepthFactorVariant1 This; | ||||
|  | @ -103,8 +106,7 @@ public: | |||
| 
 | ||||
|   /// Evaluate error h(x)-z and optionally derivatives
 | ||||
|   Vector evaluateError(const Pose3& pose, const Vector6& landmark, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
| 
 | ||||
|     if (H1) { | ||||
|       (*H1) = numericalDerivative11<Vector, Pose3>( | ||||
|  |  | |||
|  | @ -36,7 +36,10 @@ protected: | |||
| public: | ||||
| 
 | ||||
|   /// shorthand for base class type
 | ||||
|   typedef NoiseModelFactorN<Pose3, Vector3> Base; | ||||
|   typedef NoiseModelFactor2<Pose3, Vector3> Base; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// shorthand for this class
 | ||||
|   typedef InvDepthFactorVariant2 This; | ||||
|  | @ -106,8 +109,7 @@ public: | |||
| 
 | ||||
|   /// Evaluate error h(x)-z and optionally derivatives
 | ||||
|   Vector evaluateError(const Pose3& pose, const Vector3& landmark, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
| 
 | ||||
|     if (H1) { | ||||
|       (*H1) = numericalDerivative11<Vector, Pose3>( | ||||
|  |  | |||
|  | @ -34,7 +34,10 @@ protected: | |||
| public: | ||||
| 
 | ||||
|   /// shorthand for base class type
 | ||||
|   typedef NoiseModelFactorN<Pose3, Vector3> Base; | ||||
|   typedef NoiseModelFactor2<Pose3, Vector3> Base; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
|   /// shorthand for this class
 | ||||
|   typedef InvDepthFactorVariant3a This; | ||||
|  | @ -106,8 +109,7 @@ public: | |||
| 
 | ||||
|   /// Evaluate error h(x)-z and optionally derivatives
 | ||||
|   Vector evaluateError(const Pose3& pose, const Vector3& landmark, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2) const override { | ||||
| 
 | ||||
|     if(H1) { | ||||
|       (*H1) = numericalDerivative11<Vector, Pose3>( | ||||
|  | @ -232,9 +234,7 @@ public: | |||
| 
 | ||||
|   /// Evaluate error h(x)-z and optionally derivatives
 | ||||
|   Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none, | ||||
|       boost::optional<Matrix&> H3=boost::none) const override { | ||||
|       OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override { | ||||
| 
 | ||||
|     if(H1) | ||||
|       (*H1) = numericalDerivative11<Vector, Pose3>( | ||||
|  |  | |||
|  | @ -24,8 +24,8 @@ void LocalOrientedPlane3Factor::print(const string& s, | |||
| //***************************************************************************
 | ||||
| Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi, | ||||
|     const Pose3& wTwa, const OrientedPlane3& a_plane, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, | ||||
|     boost::optional<Matrix&> H3) const { | ||||
|     OptionalMatrixType H1, OptionalMatrixType H2, | ||||
|     OptionalMatrixType H3) const { | ||||
| 
 | ||||
|   Matrix66 aTai_H_wTwa, aTai_H_wTwi; | ||||
|   Matrix36 predicted_H_aTai; | ||||
|  |  | |||
Some files were not shown because too many files have changed in this diff Show More
		Loading…
	
		Reference in New Issue