diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index a59a34d27..14f2abd1a 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -2,6 +2,8 @@ class UnaryFactor: public NoiseModelFactor1 { double mx_, my_; ///< X and Y measurements public: + + // Provide access to the Matrix& version of evaluateError: using gtsam::NoiseModelFactor1::evaluateError; UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index 8cc243583..d69a0e0a4 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -53,7 +53,6 @@ class MagPoseFactor: public NoiseModelFactorN { // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - ~MagPoseFactor() override {} /// Default constructor - only use for serialization. diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 4fc98944f..e65f5d583 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -36,13 +36,13 @@ 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. -*/ + * 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(nullptr) /** This typedef will be used everywhere boost::optional reference was used @@ -53,7 +53,8 @@ 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. */ + * unwhitenedError. + */ using OptionalMatrixVecType = std::vector*; /** @@ -254,10 +255,11 @@ public: 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; */ + * 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& H) const { return unwhitenedError(x, &H); } @@ -613,12 +615,13 @@ protected: virtual Vector evaluateError(const ValueTypes&... x, OptionalMatrixTypeT... H) const = 0; - // If someone uses the evaluateError function by supplying all the optional - // arguments 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::evaluateError; + /** 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::evaluateError; + */ Vector evaluateError(const ValueTypes&... x, MatrixTypeT&... H) const { return evaluateError(x..., (&H)...); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 28fd2628f..b263b4de6 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -52,7 +52,7 @@ namespace gtsam { public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 8a780dfb0..529af878d 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -114,7 +114,6 @@ 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 shared_ptr; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 5ab06adc2..f3e27f7e5 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -112,7 +112,6 @@ 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 shared_ptr; diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index f151dd63e..73f2ceb47 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -57,7 +57,6 @@ 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 shared_ptr; diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index f1f118c2d..40fc9da25 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -99,7 +99,6 @@ 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 shared_ptr; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 3891cc41c..c620bbc87 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -39,7 +39,6 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - /// shorthand for this class typedef InvDepthFactor3 This; diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 7372e9ebb..af0bcba55 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -39,7 +39,6 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - /// shorthand for this class typedef InvDepthFactorVariant1 This; diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 8284cd9ba..b6ce1a890 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -41,7 +41,6 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - /// shorthand for this class typedef InvDepthFactorVariant2 This; diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index e90a2e609..e8c2781d4 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -39,7 +39,6 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - /// shorthand for this class typedef InvDepthFactorVariant3a This; diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 991ff4264..5745536e0 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -186,6 +186,9 @@ namespace gtsam { } /* ************************************************************************* */ + /** A function overload to accept a vector instead of a pointer to + * the said type. + */ gtsam::Vector whitenedError(const gtsam::Values& x, std::vector& H) const { return whitenedError(x, &H); } diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index fabc40810..d28dd32cd 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -179,6 +179,9 @@ namespace gtsam { /* ************************************************************************* */ + /** A function overload to accept a vector instead of a pointer to + * the said type. + */ Vector whitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const { bool debug = true;