diff --git a/examples/LocalizationExample2.cpp b/examples/LocalizationExample2.cpp index 9a4330181..ea651008c 100644 --- a/examples/LocalizationExample2.cpp +++ b/examples/LocalizationExample2.cpp @@ -47,8 +47,6 @@ public: if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0); return Vector_(2, q.x() - mx_, q.y() - my_); } - - ADD_CLONE_NONLINEAR_FACTOR(UnaryFactor) }; /** diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e9710c780..e5f419ff4 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -161,7 +161,10 @@ namespace gtsam { return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model)); } - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// @} @@ -212,7 +215,10 @@ namespace gtsam { virtual ~NonlinearEquality1() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** g(x) with optional derivative */ Vector evaluateError(const X& x1, boost::optional H = boost::none) const { @@ -269,7 +275,10 @@ namespace gtsam { : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} virtual ~NonlinearEquality2() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 8251bff59..a01cead24 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -36,9 +36,10 @@ /** * Macro to add a standard clone function to a derived factor + * @DEPRECIATED: will go away shortly - just add the clone function directly */ #define ADD_CLONE_NONLINEAR_FACTOR(Derived) \ - virtual gtsam::NonlinearFactor::shared_ptr clone() const { \ + virtual gtsam::NonlinearFactor::shared_ptr clone() const { \ return boost::static_pointer_cast( \ gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); } diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index f37debd54..163633115 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -163,7 +163,10 @@ namespace gtsam { return linearize(z_, u, p, j1, j2); } - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// @} diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index 47e0fd40c..1e0d64db7 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -50,7 +50,10 @@ namespace gtsam { virtual ~AntiFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index edd484e36..251e37a37 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -55,7 +55,10 @@ namespace gtsam { virtual ~BearingFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** h(x)-z -> between(z,h(x)) for Rot2 manifold */ Vector evaluateError(const Pose& pose, const Point& point, diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 0bfa86540..3ce671292 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -57,7 +57,10 @@ namespace gtsam { virtual ~BearingRangeFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** Print */ virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 62f66cc48..1779bf286 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -62,7 +62,10 @@ namespace gtsam { virtual ~BetweenFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d64585b7d..f4615c334 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -62,7 +62,10 @@ namespace gtsam { virtual ~GeneralSFMFactor() {} ///< destructor - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** * print diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 215f87ef7..bea3f3f46 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -87,7 +87,10 @@ namespace gtsam { this->fillH(); } - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 608e717dd..094065336 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -56,7 +56,10 @@ namespace gtsam { Base(model, key), prior_(prior) { } - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index c6529a34f..5a871d96f 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -70,7 +70,10 @@ namespace gtsam { /** Virtual destructor */ virtual ~GenericProjectionFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** * print diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index ee3dc0b8e..dcc94476a 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -51,7 +51,10 @@ namespace gtsam { virtual ~RangeFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** h(x)-z */ Vector evaluateError(const Pose& pose, const Point& point, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 500009ab4..d0c0bda6a 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -57,7 +57,10 @@ public: virtual ~GenericStereoFactor() {} ///< Virtual destructor - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** * print diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 306ec5191..c9a75ec23 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -51,11 +51,14 @@ public: virtual ~FullIMUFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { - const This* const f = dynamic_cast(&e); + const This* const f = dynamic_cast(&e); return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(gyro_, f->gyro_, tol) && diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index b1ccdb3bc..adcf772e3 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -44,11 +44,14 @@ public: virtual ~IMUFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { - const This* const f = dynamic_cast(&e); + const This* const f = dynamic_cast(&e); return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(gyro_, f->gyro_, tol) && diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 7646b7486..801bf12cd 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -72,7 +72,10 @@ public: virtual ~VelocityConstraint() {} - ADD_CLONE_NONLINEAR_FACTOR(VelocityConstraint) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); } /** * Calculates the error for trapezoidal model given diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 3fa46701d..8f9461d02 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -59,7 +59,10 @@ public: virtual ~LinearizedFactor() {} - ADD_CLONE_NONLINEAR_FACTOR(This); + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** * Use this constructor with pre-constructed decoders diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 403fac109..c5a62bba9 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -135,7 +135,10 @@ namespace simulated2D { virtual ~GenericPrior() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } private: @@ -178,7 +181,10 @@ namespace simulated2D { virtual ~GenericOdometry() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } private: @@ -222,7 +228,10 @@ namespace simulated2D { virtual ~GenericMeasurement() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } private: diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index cf0266d86..b9f2edcb5 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -59,7 +59,10 @@ namespace simulated2D { virtual ~ScalarCoordConstraint1() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** * Constructor for constraint @@ -69,8 +72,8 @@ namespace simulated2D { * @param mu is the penalty function gain */ ScalarCoordConstraint1(Key key, double c, - bool isGreaterThan, double mu = 1000.0) : - Base(key, c, isGreaterThan, mu) { + bool isGreaterThan, double mu = 1000.0) : + Base(key, c, isGreaterThan, mu) { } /** @@ -124,7 +127,10 @@ namespace simulated2D { virtual ~MaxDistanceConstraint() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** * Primary constructor for factor @@ -163,18 +169,21 @@ namespace simulated2D { */ template struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor - typedef MinDistanceConstraint This; ///< This class for factor - typedef POSE Pose; ///< Type of pose variable constrained - typedef POINT Point; ///< Type of point variable constrained + typedef BoundingConstraint2 Base; ///< Base class for factor + typedef MinDistanceConstraint This; ///< This class for factor + typedef POSE Pose; ///< Type of pose variable constrained + typedef POINT Point; ///< Type of point variable constrained - virtual ~MinDistanceConstraint() {} + virtual ~MinDistanceConstraint() {} - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - /** - * Primary constructor for factor - * @param key1 is the first variable key + /** + * Primary constructor for factor + * @param key1 is the first variable key * @param key2 is the second variable key * @param range_bound is the minimum range allowed between the variables * @param mu is the gain for the penalty function diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index ed0d8ba1a..ef8378fc5 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -118,7 +118,10 @@ namespace simulated2DOriented { return measured_.localCoordinates(odo(x1, x2, H1, H2)); } - ADD_CLONE_NONLINEAR_FACTOR(This) + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } }; diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index b518bebe5..3dd0324dc 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -210,8 +210,6 @@ namespace example { return (h(x) - z_).vector(); } - ADD_CLONE_NONLINEAR_FACTOR(UnaryFactor) - }; } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 440119e66..c19bcd4c9 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -125,8 +125,6 @@ public: virtual ~NonlinearMotionModel() {} - ADD_CLONE_NONLINEAR_FACTOR(This) - // Calculate the next state prediction using the nonlinear function f() T f(const T& x_t0) const { @@ -269,8 +267,6 @@ public: virtual ~NonlinearMeasurementModel() {} - ADD_CLONE_NONLINEAR_FACTOR(This) - // Calculate the predicted measurement using the nonlinear function h() // Byproduct: updates Jacobian H, and noiseModel (R) Vector h(const T& x_t1) const { diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 00fa217c6..0a9c59f8b 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -257,7 +257,9 @@ public: return (Vector(1) << x1 + x2 + x3 + x4).finished(); } - ADD_CLONE_NONLINEAR_FACTOR(TestFactor4) + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); } }; /* ************************************ */ @@ -305,8 +307,6 @@ public: } return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); } - - ADD_CLONE_NONLINEAR_FACTOR(TestFactor5) }; /* ************************************ */ @@ -360,7 +360,6 @@ public: return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); } - ADD_CLONE_NONLINEAR_FACTOR(TestFactor6) }; /* ************************************ */