diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index fa7559972..37414e097 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -20,6 +20,26 @@ #include #include +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, gtsam::NonlinearFactor& factor, + const unsigned int version) {} + +// template +// void serialize(Archive& ar, gtsam::ExpressionFactor& factor, +// const unsigned int version) { +// ar& BOOST_SERIALIZATION_NVP(factor.measurement_); +//} + +// template +// void serialize(Archive& ar, Factor& factor, const unsigned int version) { +//} + +} // namespace serialization +} // namespace boost + namespace gtsam { /** @@ -36,8 +56,11 @@ class BearingFactor : public ExpressionFactor { GTSAM_CONCEPT_TESTABLE_TYPE(Measured) GTSAM_CONCEPT_POSE_TYPE(Pose) + /// Default constructor + BearingFactor() {} + public: - /** primary constructor */ + /// primary constructor BearingFactor(Key poseKey, Key pointKey, const Measured& measured, const SharedNoiseModel& model) : Base(model, measured, @@ -53,6 +76,22 @@ class BearingFactor : public ExpressionFactor { this->measurement_.print(); Base::print("", keyFormatter); } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Factor", boost::serialization::base_object(*this)); + // ar& BOOST_SERIALIZATION_NVP(this->noiseModel_); + // ar& BOOST_SERIALIZATION_NVP(measurement_); + } + + template + friend void boost::serialization::serialize(Archive& ar, Base& factor, + const unsigned int version); + }; // BearingFactor } // namespace gtsam diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index 4a9330153..b12a88c9a 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include @@ -40,6 +41,8 @@ TEST(BearingFactor, 2D) { // Create a factor double measurement(10.0); BearingFactor factor(poseKey, pointKey, measurement, model); + std::string serialized = serializeXML(factor); + cout << serialized << endl; // Set the linearization point Values values;