diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index 100f6fa2e..a59a34d27 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -3,6 +3,7 @@ class UnaryFactor: public NoiseModelFactor1 { public: using gtsam::NoiseModelFactor1::evaluateError; + UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): NoiseModelFactor1(model, j), mx_(x), my_(y) {} diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 7bd4ebee6..3e5e40374 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -46,8 +46,7 @@ public: } /// evaluate the error - Vector evaluateError(const Pose3& pose, OptionalMatrixType H = - OptionalNone) const override { + Vector evaluateError(const Pose3& pose, OptionalMatrixType H) const override { PinholeCamera camera(pose, *K_); return camera.project(P_, H, OptionalNone, OptionalNone) - p_; } diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 79258b287..cfc76786a 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -71,7 +71,9 @@ class UnaryFactor: public NoiseModelFactorN { double mx_, my_; public: + // Provide access to Matrix& version of evaluateError: using NoiseModelFactor1::evaluateError; + /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index d6f0db500..4bde4e5e7 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -92,8 +92,8 @@ public: } /// Constructor that will resize a dynamic matrix (unless already correct) - OptionalJacobian(Eigen::MatrixXd* dynamic) - : map_(nullptr) { + OptionalJacobian(Eigen::MatrixXd* dynamic) : + map_(nullptr) { if (dynamic) { dynamic->resize(Rows, Cols); // no malloc if correct size usurp(dynamic->data()); diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 0a67b6b39..e19e53a1c 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -142,6 +142,7 @@ 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 shared_ptr; diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 743bba1f4..0d3ce7695 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -84,6 +84,7 @@ public: // Provide access to the Matrix& version of evaluateError: 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 646731a09..9dea1c5ab 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -40,6 +40,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index fc12519bd..4196f5f22 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -268,6 +268,7 @@ private: PreintegratedCombinedMeasurements _PIM_; public: + // Provide access to Matrix& version of evaluateError: using Base::evaluateError; /** Shorthand for a smart pointer to a factor */ diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index da6b453eb..7b4435030 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -44,6 +44,7 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 41648674b..c71078ed6 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -270,6 +270,9 @@ private: public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + /** Default constructor - only use for serialization */ ImuFactor2() {} diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index d95409bda..9e87b8324 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -37,6 +37,7 @@ class MagFactor: public NoiseModelFactorN { const Point3 bias_; ///< bias public: + // Provide access to Matrix& version of evaluateError: using NoiseModelFactor1::evaluateError; /** @@ -94,8 +95,10 @@ class MagFactor1: public NoiseModelFactorN { const Point3 bias_; ///< bias public: + // Provide access to Matrix& version of evaluateError: using NoiseModelFactor1::evaluateError; + /** Constructor */ MagFactor1(Key key, const Point3& measured, double scale, const Unit3& direction, const Point3& bias, @@ -131,8 +134,10 @@ class MagFactor2: public NoiseModelFactorN { const Rot3 bRn_; ///< The assumed known rotation from nav to body public: + // Provide access to Matrix& version of evaluateError: using NoiseModelFactor2::evaluateError; + /** Constructor */ MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : @@ -172,8 +177,10 @@ class MagFactor3: public NoiseModelFactorN { const Rot3 bRn_; ///< The assumed known rotation from nav to body public: + // Provide access to Matrix& version of evaluateError: using NoiseModelFactor3::evaluateError; + /** Constructor */ MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index f0b82eed7..75b0aeedf 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -52,6 +52,7 @@ 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/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index e5df25ca9..28831debf 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -343,7 +343,8 @@ protected: ExpressionFactor2() {} /// Constructor takes care of keys, but still need to call initialize - ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel& noiseModel, const T& measurement) + ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel& noiseModel, + const T &measurement) : ExpressionFactorN({key1, key2}, noiseModel, measurement) {} }; // ExpressionFactor2 diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 59702ee74..0648bebfe 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -67,6 +67,7 @@ class FunctorizedFactor : public NoiseModelFactorN { public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /** default constructor - only use for serialization */ FunctorizedFactor() {} @@ -169,6 +170,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN { public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /** default constructor - only use for serialization */ FunctorizedFactor2() {} diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 309cababa..09c58ba80 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -209,6 +209,8 @@ class NonlinearEquality1: public NoiseModelFactorN { public: typedef VALUE X; + + // Provide access to Matrix& version of evaluateError: using NoiseModelFactor1::evaluateError; protected: @@ -308,6 +310,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /** * Constructor * @param key1 the key for the first unknown variable to be constrained diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 0a97ab39e..6d0fcb4fc 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -32,7 +32,8 @@ namespace gtsam { public: typedef VALUE T; // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; + using NoiseModelFactor1::evaluateError; + private: @@ -113,10 +114,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 diff --git a/gtsam/sfm/ShonanFactor.h b/gtsam/sfm/ShonanFactor.h index a186986dd..6fea8ac14 100644 --- a/gtsam/sfm/ShonanFactor.h +++ b/gtsam/sfm/ShonanFactor.h @@ -42,7 +42,9 @@ class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN { using Rot = typename std::conditional::type; public: + // Provide access to the Matrix& version of evaluateError: using NoiseModelFactor2::evaluateError; + /// @name Constructor /// @{ diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 16b9fede1..df7c8a65a 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -45,7 +45,9 @@ class TranslationFactor : public NoiseModelFactorN { Point3 measured_w_aZb_; public: + // Provide access to the Matrix& version of evaluateError: using NoiseModelFactor2::evaluateError; + /// default constructor TranslationFactor() {} diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 645fb611c..8e564ddb6 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -41,6 +41,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /** * Constructor * @param key Essential Matrix variable key @@ -119,6 +120,7 @@ class EssentialMatrixFactor2 public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /** * Constructor * @param key1 Essential Matrix variable key @@ -240,6 +242,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /** * Constructor * @param key1 Essential Matrix variable key @@ -342,6 +345,7 @@ class EssentialMatrixFactor4 public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /** * Constructor * @param keyE Essential Matrix (from camera B to A) variable key diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 18181e091..6c9027f6f 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -85,6 +85,7 @@ class FrobeniusFactor : public NoiseModelFactorN { // Provide access to the Matrix& version of evaluateError: using NoiseModelFactor2::evaluateError; + /// Constructor FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j1, @@ -113,7 +114,9 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; public: + // Provide access to the Matrix& version of evaluateError: using NoiseModelFactor2::evaluateError; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @name Constructor diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 78c7c8cd8..3f405f985 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -77,7 +77,7 @@ public: typedef NoiseModelFactorN Base;///< typedef for the base class // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError;// + using Base::evaluateError; // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 4dac8b553..4b9398cb3 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -23,6 +23,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN::evaluateError; + /// Constructor OrientedPlane3Factor() { } diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 58dfda8f9..c8dda6691 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -25,7 +25,6 @@ public: typedef typename POSE::Translation Translation; typedef typename POSE::Rotation Rotation; - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index fa21a7223..2ffafd104 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -26,7 +26,6 @@ public: typedef typename POSE::Translation Translation; typedef typename POSE::Rotation Rotation; - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 55fcfd61d..c0e3fd1b3 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -48,10 +48,9 @@ public: typedef boost::shared_ptr 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 */ diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 91905699c..db546c86c 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -25,6 +25,7 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + typedef boost::shared_ptr shared_ptr; ///TODO: comment diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index b740ba6f2..9bc8efd54 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -86,7 +86,7 @@ Pose3Upright Pose3Upright::inverse(OptionalJacobian<4, 4> H1) const { return Pose3Upright(T_.inverse(), -z_); } OptionalJacobian<3, 3>::Jacobian H3x3; - // TODO: Could not use reference to a view into H1 and reuse memory + // TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory // Eigen::Ref> H3x3 = H1->topLeftCorner(3,3); Pose3Upright result(T_.inverse(H3x3), -z_); Matrix H1_ = -I_4x4; @@ -102,7 +102,7 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, if (!H1 && !H2) return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_); - // TODO: Could not use reference to a view into H1 and reuse memory + // 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) { @@ -121,7 +121,7 @@ Pose3Upright Pose3Upright::between(const Pose3Upright& p2, if (!H1 && !H2) return Pose3Upright(T_.between(p2.T_), p2.z_ - z_); - // TODO: Could not use reference to a view into H1 and H2 to reuse memory + // 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) { diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 529af878d..8a780dfb0 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -114,6 +114,7 @@ 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 f3e27f7e5..5ab06adc2 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -112,6 +112,7 @@ 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 6dfd8d295..32755b18d 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -56,6 +56,7 @@ 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 c5fbc9a32..c1c448b97 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -98,6 +98,7 @@ 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 8db48955b..f32399c87 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -38,6 +38,7 @@ 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 c8958f2f4..fb2b12cf2 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -38,6 +38,7 @@ 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 967c9c9c5..dfe75c10e 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -40,6 +40,7 @@ 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 3685b48b9..bd373f20d 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -38,6 +38,7 @@ 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/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h index a8b42ed99..8b958da22 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -42,6 +42,7 @@ class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /// Constructor LocalOrientedPlane3Factor() {} diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index 968c56407..f45b6c6b9 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -31,6 +31,7 @@ class PoseToPointFactor : public NoiseModelFactorN { public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 5ac0c9cf0..859e44f6d 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -64,6 +64,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /// shorthand for this class typedef ProjectionFactorRollingShutter This; diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index caae63122..4dca82903 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -69,6 +69,7 @@ private: public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + /// Constructor DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured, const SharedNoiseModel& model) : diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index c9ac38f82..c9c35099d 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -106,7 +106,9 @@ namespace simulated2DOriented { Pose2 measured_; ///< Between measurement for odometry factor typedef GenericOdometry This; - using NoiseModelFactor2::evaluateError; + + // Provide access to the Matrix& version of evaluateError: + using NoiseModelFactor2::evaluateError; /** * Creates an odometry factor between two poses diff --git a/tests/smallExample.h b/tests/smallExample.h index 2030c973b..6311999e5 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -329,7 +329,9 @@ inline Matrix H(const Point2& v) { struct UnaryFactor: public gtsam::NoiseModelFactorN { + // Provide access to the Matrix& version of evaluateError: using gtsam::NoiseModelFactor1::evaluateError; + Point2 z_; UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index feeb4c43f..eb17ecd9e 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -249,6 +249,7 @@ protected: public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + NonlinearMeasurementModel(){} NonlinearMeasurementModel(const Symbol& TestKey, Vector z) : diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index cbfad704c..e9e755044 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -337,8 +337,11 @@ class TestFactor1 : public NoiseModelFactor1 { public: typedef NoiseModelFactor1 Base; + // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} + // Provide access to the Matrix& version of evaluateError: using Base::NoiseModelFactor1; // inherit constructors Vector evaluateError(const double& x1, OptionalMatrixType H1) const override { @@ -391,7 +394,9 @@ class TestFactor4 : public NoiseModelFactor4 { typedef NoiseModelFactor4 Base; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + // Provide access to the Matrix& version of evaluateError: using Base::NoiseModelFactor4; // inherit constructors Vector @@ -482,6 +487,7 @@ public: typedef NoiseModelFactor5 Base; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {} Vector @@ -531,6 +537,7 @@ public: typedef NoiseModelFactor6 Base; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {} Vector @@ -587,6 +594,7 @@ public: typedef NoiseModelFactorN Base; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; + using Type1 = ValueType<1>; // Test that we can use the ValueType<> template TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}