diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4af803711..5d6d28061 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -23,10 +23,10 @@ #include #include #include - namespace gtsam { /** + * Factor that supports arbitrary expressions via AD */ template @@ -132,10 +132,10 @@ public: } protected: - /// Default constructor, for serialization ExpressionFactor() {} + /// Default constructor, for serialization - /// Constructor for use by SerializableExpressionFactor + /// Constructor for serializable derived classes ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement) : NoiseModelFactor(noiseModel), measured_(measurement) { // Not properly initialized yet, need to call initialize @@ -155,22 +155,91 @@ protected: boost::tie(keys_, dims_) = expression_.keysAndDims(); } -private: - /// Serialization function - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor); - ar& boost::serialization::make_nvp("measured_", this->measured_); + /// Recreate expression from keys_ and measured_, used in load below. + /// Needed to deserialize a derived factor + virtual Expression expression() const { + throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize."); } +private: + /// Save to an archive: just saves the base class + template + void save(Archive& ar, const unsigned int /*version*/) const { + ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor); + ar << boost::serialization::make_nvp("measured_", this->measured_); + } + + /// Load from an archive, creating a valid expression using the overloaded + /// [expression] method + template + void load(Archive& ar, const unsigned int /*version*/) { + ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor); + ar >> boost::serialization::make_nvp("measured_", this->measured_); + this->initialize(expression()); + } + + // Indicate that we implement save/load separately, and be friendly to boost + BOOST_SERIALIZATION_SPLIT_MEMBER() + friend class boost::serialization::access; }; // ExpressionFactor - /// traits template struct traits > : public Testable > {}; +/** + * Binary specialization of ExpressionFactor meant as a base class for binary factors + * Enforces expression method with two keys, and provides evaluateError + * Derived 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 + virtual Expression expression() const { + return expression(this->keys_[0], this->keys_[1]); + } +}; +// ExpressionFactor2 + }// \ namespace gtsam diff --git a/gtsam/nonlinear/SerializableExpressionFactor.h b/gtsam/nonlinear/SerializableExpressionFactor.h deleted file mode 100644 index 797bce4fd..000000000 --- a/gtsam/nonlinear/SerializableExpressionFactor.h +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file SerializableExpressionFactor.h - * @date July 2015 - * @author Frank Dellaert - * @brief ExpressionFactor variant that supports serialization - */ - -#pragma once - -#include - -namespace gtsam { - -/** - * ExpressionFactor variant that supports serialization - * Simply overload the pure virtual method [expression] to construct an - * expression from keys_ - */ -template -class SerializableExpressionFactor : public ExpressionFactor { - public: - /// Constructor takes only two arguments, still need to call initialize - SerializableExpressionFactor(const SharedNoiseModel& noiseModel, - const T& measurement) - : ExpressionFactor(noiseModel, measurement) {} - - /// Destructor - virtual ~SerializableExpressionFactor() {} - - protected: - /// Return an expression that predicts the measurement given Values - virtual Expression expression() const = 0; - - /// Default constructor, for serialization - SerializableExpressionFactor() {} - - /// Save to an archive: just saves the base class - template - void save(Archive& ar, const unsigned int /*version*/) const { - ar << boost::serialization::make_nvp( - "ExpressionFactor", - boost::serialization::base_object >(*this)); - } - - /// Load from an archive, creating a valid expression using the overloaded - /// [expression] method - template - void load(Archive& ar, const unsigned int /*version*/) { - ar >> boost::serialization::make_nvp( - "ExpressionFactor", - boost::serialization::base_object >(*this)); - this->initialize(expression()); - } - - // Indicate that we implement save/load separately, and be friendly to boost - BOOST_SERIALIZATION_SPLIT_MEMBER() - friend class boost::serialization::access; -}; -// SerializableExpressionFactor - -/** - * Binary specialization of SerializableExpressionFactor - * Enforces expression method with two keys, and provides evaluateError - */ -template -class SerializableExpressionFactor2 : public SerializableExpressionFactor { - public: - /// Constructor takes care of keys, but still need to call initialize - SerializableExpressionFactor2(Key key1, Key key2, - const SharedNoiseModel& noiseModel, - const T& measurement) - : SerializableExpressionFactor(noiseModel, measurement) { - this->keys_.push_back(key1); - this->keys_.push_back(key2); - } - - /// Destructor - virtual ~SerializableExpressionFactor2() {} - - /// 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; - } - - /// Return an expression that predicts the measurement given Values - virtual Expression expression(Key key1, Key key2) const = 0; - - protected: - /// Default constructor, for serialization - SerializableExpressionFactor2() {} - - private: - /// Return an expression that predicts the measurement given Values - virtual Expression expression() const { - return expression(this->keys_[0], this->keys_[1]); - } -}; -// SerializableExpressionFactor2 - -} // \ namespace gtsam diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index d0d06d30a..f190e683c 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -34,8 +34,8 @@ struct Bearing; */ template ::result_type> -struct BearingFactor : public SerializableExpressionFactor2 { - typedef SerializableExpressionFactor2 Base; +struct BearingFactor : public ExpressionFactor2 { + typedef ExpressionFactor2 Base; /// default constructor BearingFactor() {} diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 1f4122255..2dd1fecb8 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include #include @@ -33,10 +33,10 @@ template ::result_type, typename R = typename Range::result_type> class BearingRangeFactor - : public SerializableExpressionFactor2, A1, A2> { + : public ExpressionFactor2, A1, A2> { private: typedef BearingRange T; - typedef SerializableExpressionFactor2 Base; + typedef ExpressionFactor2 Base; typedef BearingRangeFactor This; public: @@ -62,7 +62,8 @@ class BearingRangeFactor // Return measurement expression virtual Expression expression(Key key1, Key key2) const { - return Expression(T::Measure, Expression(key1), Expression(key2)); + return Expression(T::Measure, Expression(key1), + Expression(key2)); } /// print diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 6e1a33481..f78913448 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -33,10 +33,10 @@ struct Range; */ template ::result_type> -class RangeFactor : public SerializableExpressionFactor2 { +class RangeFactor : public ExpressionFactor2 { private: typedef RangeFactor This; - typedef SerializableExpressionFactor2 Base; + typedef ExpressionFactor2 Base; public: /// default constructor @@ -79,11 +79,10 @@ struct traits > */ template ::result_type> -class RangeFactorWithTransform - : public SerializableExpressionFactor2 { +class RangeFactorWithTransform : public ExpressionFactor2 { private: typedef RangeFactorWithTransform This; - typedef SerializableExpressionFactor2 Base; + typedef ExpressionFactor2 Base; A1 body_T_sensor_; ///< The pose of the sensor in the body frame