diff --git a/CMakeLists.txt b/CMakeLists.txt index efa4100a8..f0308a8f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ endif() option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON) add_definitions(-Wno-deprecated-declarations) +add_definitions(-ftemplate-backtrace-limit=0) set(CMAKE_CXX_STANDARD 17) if (GTSAM_NO_BOOST_CPP17) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 4da01ec15..37bc29164 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -82,6 +82,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public At public: + using Base::evaluateError; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -156,6 +157,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, typedef NoiseModelFactorN Base; public: + using Base::evaluateError; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index be9b84f77..a72b87024 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -38,6 +38,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { double nT_; ///< Height Measurement based on a standard atmosphere public: + using Base::evaluateError; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -76,10 +77,8 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { double tol = 1e-9) const override; /// vector of errors - Vector evaluateError( - const Pose3& p, const double& b, - OptionalMatrixType H = OptionalNone, - OptionalMatrixType H2 = OptionalNone) const override; + Vector evaluateError(const Pose3& p, const double& b, + OptionalMatrixType H, OptionalMatrixType H2) const override; inline const double& measurementIn() const { return nT_; } diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index ccff88cf2..536268e5c 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -30,7 +30,8 @@ class ConstantVelocityFactor : public NoiseModelFactorN { double dt_; public: - using Base = NoiseModelFactorN; + using Base = NoiseModelFactor2; + using Base::evaluateError; public: ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) @@ -48,8 +49,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN { * @return * Vector */ gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) 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}; diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index f00ead70c..51b86a00f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -179,6 +179,7 @@ private: PreintegratedImuMeasurements _PIM_; public: + using Base::evaluateError; /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 987869702..4b117d2b4 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -87,8 +87,7 @@ class FunctorizedFactor : public NoiseModelFactorN { NonlinearFactor::shared_ptr(new FunctorizedFactor(*this))); } - Vector evaluateError(const T ¶ms, OptionalMatrixType H = - OptionalNone) const override { + Vector evaluateError(const T ¶ms, OptionalMatrixType H) const override { R x = func_(params, H); Vector error = traits::Local(measured_, x); return error; @@ -193,8 +192,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN { Vector evaluateError( const T1 ¶ms1, const T2 ¶ms2, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) const override { + OptionalMatrixType H1, OptionalMatrixType H2) const override { R x = func_(params1, params2, H1, H2); Vector error = traits::Local(measured_, x); return error; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 7f63639dc..db71556a1 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -46,6 +46,7 @@ class NonlinearEquality: public NoiseModelFactorN { public: typedef VALUE T; + using NoiseModelFactor1::evaluateError; private: diff --git a/gtsam/sfm/ShonanFactor.h b/gtsam/sfm/ShonanFactor.h index faa88879f..a186986dd 100644 --- a/gtsam/sfm/ShonanFactor.h +++ b/gtsam/sfm/ShonanFactor.h @@ -42,6 +42,7 @@ class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN { using Rot = typename std::conditional::type; public: + using NoiseModelFactor2::evaluateError; /// @name Constructor /// @{ @@ -71,10 +72,7 @@ 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, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) const override; + Vector evaluateError(const SOn& Q1, const SOn& Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override; /// @} private: diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 3fc0837ba..16b9fede1 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -45,6 +45,7 @@ class TranslationFactor : public NoiseModelFactorN { Point3 measured_w_aZb_; public: + using NoiseModelFactor2::evaluateError; /// default constructor TranslationFactor() {} @@ -65,8 +66,8 @@ class TranslationFactor : public NoiseModelFactorN { */ Vector evaluateError( const Point3& Ta, const Point3& Tb, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) 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); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e1d3395a2..6e5959df4 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -38,6 +38,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { typedef EssentialMatrixFactor This; public: + using Base::evaluateError; /** * Constructor * @param key Essential Matrix variable key @@ -90,8 +91,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { /// vector of errors returns 1D vector Vector evaluateError( - const EssentialMatrix& E, - OptionalMatrixType H = OptionalNone) const override { + const EssentialMatrix& E, OptionalMatrixType H) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; @@ -115,6 +115,7 @@ class EssentialMatrixFactor2 typedef EssentialMatrixFactor2 This; public: + using Base::evaluateError; /** * Constructor * @param key1 Essential Matrix variable key @@ -173,8 +174,7 @@ class EssentialMatrixFactor2 */ Vector evaluateError( const EssentialMatrix& E, const double& d, - OptionalMatrixType DE = OptionalNone, - OptionalMatrixType Dd = OptionalNone) 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 +235,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { Rot3 cRb_; ///< Rotation from body to camera frame public: + using Base::evaluateError; /** * Constructor * @param key1 Essential Matrix variable key @@ -284,8 +285,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { */ Vector evaluateError( const EssentialMatrix& E, const double& d, - OptionalMatrixType DE = OptionalNone, - OptionalMatrixType Dd = OptionalNone) const override { + OptionalMatrixType DE, OptionalMatrixType Dd) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -332,6 +332,7 @@ class EssentialMatrixFactor4 typedef Eigen::Matrix JacobianCalibration; public: + using Base::evaluateError; /** * Constructor * @param keyE Essential Matrix (from camera B to A) variable key @@ -372,8 +373,7 @@ class EssentialMatrixFactor4 */ Vector evaluateError( const EssentialMatrix& E, const CALIBRATION& K, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) 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 diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 9521b1fb2..4bd1bf7d9 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -54,6 +54,7 @@ class FrobeniusPrior : public NoiseModelFactorN { Eigen::Matrix vecM_; ///< vectorized matrix to approximate public: + using NoiseModelFactor1::evaluateError; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Constructor @@ -64,8 +65,7 @@ class FrobeniusPrior : public NoiseModelFactorN { } /// Error is just Frobenius norm between Rot element and vectorized matrix M. - Vector evaluateError(const Rot& R, - OptionalMatrixType H = OptionalNone) const override { + Vector evaluateError(const Rot& R, OptionalMatrixType H) const override { return R.vec(H) - vecM_; // Jacobian is computed only when needed. } }; @@ -108,6 +108,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; public: + using NoiseModelFactor2::evaluateError; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @name Constructor diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 9258a8e4e..9072d8e6e 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -62,6 +62,7 @@ protected: public: typedef NoiseModelFactorN Base; typedef ReferenceFrameFactor This; + using Base::evaluateError; typedef POINT Point; typedef TRANSFORM Transform; @@ -95,9 +96,8 @@ public: /** Combined cost and derivative function using boost::optional */ Vector evaluateError(const Point& global, const Transform& trans, const Point& local, - OptionalMatrixType Dforeign = OptionalNone, - OptionalMatrixType Dtrans = OptionalNone, - OptionalMatrixType Dlocal = OptionalNone) const override { + OptionalMatrixType Dforeign, OptionalMatrixType Dtrans, + OptionalMatrixType Dlocal) const override { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) { *Dlocal = -1* Matrix::Identity(traits::dimension, traits::dimension);