diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 8c4304318..a690a62b3 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include #include @@ -226,68 +227,6 @@ private: template struct traits > : public Testable > {}; -/** - * Binary specialization of ExpressionFactor meant as a base class for binary - * factors. Enforces an 'expression' method with two keys, and provides 'evaluateError'. - * Derived class (a binary factor!) needs to call 'initialize'. - */ -template -class ExpressionFactor2 : public ExpressionFactor { - public: - /// Destructor - virtual ~ExpressionFactor2() {} - - /// Backwards compatible evaluateError, to make existing tests compile - Vector evaluateError(const A1& a1, const A2& a2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - Values values; - values.insert(this->keys_[0], a1); - values.insert(this->keys_[1], a2); - std::vector H(2); - Vector error = this->unwhitenedError(values, H); - if (H1) (*H1) = H[0]; - if (H2) (*H2) = H[1]; - return error; - } - - /// Recreate expression from given keys_ and measured_, used in load - /// Needed to deserialize a derived factor - virtual Expression expression(Key key1, Key key2) const { - throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize."); - } - - protected: - /// Default constructor, for serialization - ExpressionFactor2() {} - - /// Constructor takes care of keys, but still need to call initialize - ExpressionFactor2(Key key1, Key key2, - const SharedNoiseModel& noiseModel, - const T& measurement) - : ExpressionFactor(noiseModel, measurement) { - this->keys_.push_back(key1); - this->keys_.push_back(key2); - } - - private: - /// Return an expression that predicts the measurement given Values - Expression expression() const override { - return expression(this->keys_[0], this->keys_[1]); - } - - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "ExpressionFactor", boost::serialization::base_object >(*this)); - } -}; -/// traits -template -struct traits> : public Testable> {}; -// ExpressionFactor2 - /** * N-ary variadic template for ExpressionFactor meant as a base class for N-ary * factors. Enforces an 'expression' method with N keys. @@ -355,4 +294,58 @@ struct traits> : public Testable> {}; // ExpressionFactorN + +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) +/** + * Binary specialization of ExpressionFactor meant as a base class for binary + * factors. Enforces an 'expression' method with two keys, and provides + * 'evaluateError'. Derived class (a binary factor!) needs to call 'initialize'. + * + * \sa ExpressionFactorN + * \deprecated Prefer the more general ExpressionFactorN<>. + */ +template +class ExpressionFactor2 : public ExpressionFactorN { +public: + /// Destructor + ~ExpressionFactor2() override {} + + /// Backwards compatible evaluateError, to make existing tests compile + Vector evaluateError(const A1 &a1, const A2 &a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Values values; + values.insert(this->keys_[0], a1); + values.insert(this->keys_[1], a2); + std::vector H(2); + Vector error = ExpressionFactor::unwhitenedError(values, H); + if (H1) (*H1) = H[0]; + if (H2) (*H2) = H[1]; + return error; + } + + /// Recreate expression from given keys_ and measured_, used in load + /// Needed to deserialize a derived factor + virtual Expression expression(Key key1, Key key2) const { + throw std::runtime_error( + "ExpressionFactor2::expression not provided: cannot deserialize."); + } + virtual Expression + expression(const typename ExpressionFactorN::ArrayNKeys &keys) + const override { + return expression(keys[0], keys[1]); + } + +protected: + /// Default constructor, for serialization + ExpressionFactor2() {} + + /// Constructor takes care of keys, but still need to call initialize + ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel, + const T &measurement) + : ExpressionFactorN({key1, key2}, noiseModel, measurement) {} +}; +// ExpressionFactor2 +#endif + } // namespace gtsam