diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index f30392d62..0b6dde95d 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -25,6 +25,7 @@ class RotateFactor: public NoiseModelFactor1 { Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z typedef NoiseModelFactor1 Base; + typedef RotateFactor This; public: @@ -34,6 +35,11 @@ public: Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) { } + /// @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 { @@ -63,6 +69,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1 { Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z typedef NoiseModelFactor1 Base; + typedef RotateDirectionsFactor This; public: @@ -72,6 +79,11 @@ public: Base(model, key), p_(p), z_(z) { } + /// @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 {