diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 605ea5738..dd8b867be 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -40,8 +40,10 @@ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) /* ************************************************************************ */ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { const This *e = dynamic_cast(&lf); - // TODO(Varun) How to compare inner_ when they are abstract types? - return e != nullptr && Base::equals(*e, tol); + if (e == nullptr) return false; + if (!Base::equals(*e, tol)) return false; + return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false) + : !(e->inner_); } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 7ac97443a..7a43ab3a5 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -45,6 +45,9 @@ class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor { /// @name Constructors /// @{ + /// Default constructor - for serialization. + HybridDiscreteFactor() = default; + // Implicit conversion from a shared ptr of DF HybridDiscreteFactor(DiscreteFactor::shared_ptr other); @@ -70,6 +73,15 @@ class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor { /// Return the error of the underlying Discrete Factor. double error(const HybridValues &values) const override; /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(inner_); + } }; // traits diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 5a89a04a8..4fe18bea7 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -44,15 +44,21 @@ HybridGaussianFactor::HybridGaussianFactor(HessianFactor &&hf) /* ************************************************************************* */ bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { const This *e = dynamic_cast(&other); - // TODO(Varun) How to compare inner_ when they are abstract types? - return e != nullptr && Base::equals(*e, tol); + if (e == nullptr) return false; + if (!Base::equals(*e, tol)) return false; + return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false) + : !(e->inner_); } /* ************************************************************************* */ void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner_->print("\n", formatter); + if (inner_) { + inner_->print("\n", formatter); + } else { + std::cout << "\nGaussian: nullptr" << std::endl; + } }; /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6d179d1c1..6bb022396 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -43,6 +43,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { using This = HybridGaussianFactor; using shared_ptr = boost::shared_ptr; + /// @name Constructors + /// @{ + + /// Default constructor - for serialization. HybridGaussianFactor() = default; /** @@ -79,7 +83,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { */ explicit HybridGaussianFactor(HessianFactor &&hf); - public: + /// @} /// @name Testable /// @{ @@ -101,6 +105,15 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// Return the error of the underlying Gaussian factor. double error(const HybridValues &values) const override; /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(inner_); + } }; // traits