diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 064a116a1..5de62342f 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -21,8 +21,6 @@ #include #include -//#include -//#include #include #include @@ -60,6 +58,7 @@ namespace gtsam { void assertInvariants() const; public: + typedef BayesNet Base; typedef typename boost::shared_ptr shared_ptr; typedef typename boost::weak_ptr weak_ptr; weak_ptr parent_; @@ -123,6 +122,18 @@ namespace gtsam { /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); + 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(parent_); + ar & BOOST_SERIALIZATION_NVP(children_); + ar & BOOST_SERIALIZATION_NVP(separator_); + ar & BOOST_SERIALIZATION_NVP(cachedFactor_); + } + }; // \struct Clique // typedef for shared pointers to cliques @@ -313,6 +324,15 @@ namespace gtsam { template void removeTop(const CONTAINER& keys, BayesNet& bn, Cliques& orphans); + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(nodes_); + ar & BOOST_SERIALIZATION_NVP(root_); + } + }; // BayesTree } /// namespace gtsam diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index fa870537b..6d8ef2bb5 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -163,6 +163,7 @@ private: 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(nrFrontals_); } }; diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index ec2387bf7..c07b90c9c 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -67,7 +67,6 @@ public: /** Const iterator over keys */ typedef typename std::vector::const_iterator const_iterator; -//private: // FIXME: factors that manually initialize keys_ break in this case protected: // The keys involved in this factor diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index c2b76220e..2f34969ee 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -130,6 +130,14 @@ namespace gtsam { virtual ~IndexFactor() { } + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; // IndexFactor } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 9886d144a..6fb25c106 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -94,6 +94,14 @@ namespace gtsam { */ virtual void permuteWithInverse(const Permutation& inversePermutation) = 0; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexFactor); + } + }; // GaussianFactor /** make keys from list, vector, or map of matrices */ diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 1e3ac8fd6..49a6cdac8 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -71,6 +71,7 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; + typedef FactorGraph Base; /** * Default constructor @@ -165,6 +166,14 @@ namespace gtsam { // // allocate a vectorvalues of b's structure // VectorValues allocateVectorValuesb() const ; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; /** diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 06a73e80a..3e2ac28b6 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -131,11 +131,20 @@ public: /** equals (from Testable) for testing and debugging */ bool equals(const Ordering& rhs, double tol = 0.0) const; +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(order_); + ar & BOOST_SERIALIZATION_NVP(nVars_); + } }; /** * @class Unordered - * @brief a set of unordered indice + * @brief a set of unordered indices */ class Unordered: public std::set, public Testable { public: diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index ed74b6f5f..8f374f904 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -66,7 +66,9 @@ namespace gtsam { * Unary factor encoding a soft prior on a vector */ template - struct GenericPrior: public NonlinearFactor1 { + class GenericPrior: public NonlinearFactor1 { + public: + typedef NonlinearFactor1 Base; typedef boost::shared_ptr > shared_ptr; typedef typename KEY::Value Pose; Pose z_; @@ -80,13 +82,24 @@ namespace gtsam { return (prior(x, H) - z_).vector(); } + private: + GenericPrior() {} + /** 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(z_); + } }; /** * Binary factor simulating "odometry" between two Vectors */ template - struct GenericOdometry: public NonlinearFactor2 { + class GenericOdometry: public NonlinearFactor2 { + public: + typedef NonlinearFactor2 Base; typedef boost::shared_ptr > shared_ptr; typedef typename KEY::Value Pose; Pose z_; @@ -101,6 +114,15 @@ namespace gtsam { return (odo(x1, x2, H1, H2) - z_).vector(); } + private: + GenericOdometry() {} + /** 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(z_); + } }; /** @@ -109,6 +131,7 @@ namespace gtsam { template class GenericMeasurement: public NonlinearFactor2 { public: + typedef NonlinearFactor2 Base; typedef boost::shared_ptr > shared_ptr; typedef typename XKEY::Value Pose; typedef typename LKEY::Value Point; @@ -117,7 +140,7 @@ namespace gtsam { GenericMeasurement(const Point& z, const SharedGaussian& model, const XKEY& i, const LKEY& j) : - NonlinearFactor2 (model, i, j), z_(z) { + NonlinearFactor2 (model, i, j), z_(z) { } Vector evaluateError(const Pose& x1, const Point& x2, boost::optional< @@ -125,6 +148,15 @@ namespace gtsam { return (mea(x1, x2, H1, H2) - z_).vector(); } + private: + GenericMeasurement() {} + /** 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(z_); + } }; /** Typedefs for regular use */ diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index bcbab4480..827f8bc57 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -9,7 +9,7 @@ * -------------------------------------------------------------------------- */ -/* +/** * @brief Unit tests for serialization of library classes * @author Frank Dellaert * @author Alex Cunningham @@ -30,6 +30,8 @@ #include #include #include +#include +#include #include #include @@ -86,25 +88,30 @@ bool equalsDereferenced(const T& input) { } /* ************************************************************************* */ +template +std::string serializeXML(const T& input) { + std::ostringstream out_archive_stream; + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp("data", input); + return out_archive_stream.str(); +} + +template +void deserializeXML(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp("data", output); +} + // Templated round-trip serialization using XML template void roundtripXML(const T& input, T& output) { // Serialize - std::string serialized; - { - std::ostringstream out_archive_stream; - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp("data", input); - - // Convert to string - serialized = out_archive_stream.str(); - } + std::string serialized = serializeXML(input); if (verbose) std::cout << serialized << std::endl << std::endl; // De-serialize - std::istringstream in_archive_stream(serialized); - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp("data", output); + deserializeXML(serialized, output); } // This version requires equality operator @@ -135,6 +142,8 @@ bool equalsDereferencedXML(const T& input = T()) { // Actual Tests /* ************************************************************************* */ +#define GTSAM_MAGIC_KEY + #include #include #include @@ -303,9 +312,11 @@ TEST (Serialization, SharedDiagonal_noiseModels) { #include #include #include +#include +#include /* ************************************************************************* */ -TEST (Serialization, linear) { +TEST (Serialization, linear_factors) { vector dims; dims.push_back(1); dims.push_back(2); @@ -326,15 +337,132 @@ TEST (Serialization, linear) { HessianFactor hessianfactor(jacobianfactor); EXPECT(equalsObj(hessianfactor)); EXPECT(equalsXML(hessianfactor)); - { - Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); - Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); - Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); - Vector d(2); d << 0.2, 0.5; - GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); -// EXPECT(equalsObj(cg)); // FAILS: does not match -// EXPECT(equalsXML(cg)); // FAILS: does not match - } +} + +/* ************************************************************************* */ +TEST (Serialization, gaussian_conditional) { + Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); + Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); + Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); + Vector d(2); d << 0.2, 0.5; + GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); + + EXPECT(equalsObj(cg)); + EXPECT(equalsXML(cg)); +} + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +/* ************************************************************************* */ +TEST (Serialization, smallExample_linear) { + using namespace example; + + Ordering ordering; ordering += "x1","x2","l1"; + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + EXPECT(equalsObj(ordering)); + EXPECT(equalsXML(ordering)); + + EXPECT(equalsObj(fg)); + EXPECT(equalsXML(fg)); + + GaussianBayesNet cbn = createSmallGaussianBayesNet(); + EXPECT(equalsObj(cbn)); + EXPECT(equalsXML(cbn)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_graph) { + Ordering o; o += "x1","l1","x2"; + // construct expected symbolic graph + SymbolicFactorGraph sfg; + sfg.push_factor(o["x1"]); + sfg.push_factor(o["x1"],o["x2"]); + sfg.push_factor(o["x1"],o["l1"]); + sfg.push_factor(o["l1"],o["x2"]); + + EXPECT(equalsObj(sfg)); + EXPECT(equalsXML(sfg)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bn) { + Ordering o; o += "x2","l1","x1"; + + // create expected Chordal bayes Net + IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); + IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); + IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); + + SymbolicBayesNet sbn; + sbn.push_back(x2); + sbn.push_back(l1); + sbn.push_back(x1); + + EXPECT(equalsObj(sbn)); + EXPECT(equalsXML(sbn)); +} + +#include +#include + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bayes_tree ) { + typedef BayesTree SymbolicBayesTree; + static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; + IndexConditional::shared_ptr + B(new IndexConditional(_B_)), + L(new IndexConditional(_L_, _B_)), + E(new IndexConditional(_E_, _L_, _B_)), + S(new IndexConditional(_S_, _L_, _B_)), + T(new IndexConditional(_T_, _E_, _L_)), + X(new IndexConditional(_X_, _E_)); + + // Bayes Tree for Asia example + SymbolicBayesTree bayesTree; + bayesTree.insert(B); + bayesTree.insert(L); + bayesTree.insert(E); + bayesTree.insert(S); + bayesTree.insert(T); + bayesTree.insert(X); + + EXPECT(equalsObj(bayesTree)); + EXPECT(equalsXML(bayesTree)); +} + +#include +#include + +/* ************************************************************************* */ +TEST (Serialization, gaussianISAM) { + using namespace example; + Ordering ordering; + GaussianFactorGraph smoother; + boost::tie(smoother, ordering) = createSmoother(7); + GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); + GaussianISAM bayesTree(chordalBayesNet); + + EXPECT(equalsObj(bayesTree)); + EXPECT(equalsXML(bayesTree)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors in simulated2D */ +BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Prior, "gtsam::simulated2D::Prior" ); +BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Odometry, "gtsam::simulated2D::Odometry" ); +BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Measurement, "gtsam::simulated2D::Measurement"); +/* ************************************************************************* */ +TEST (Serialization, smallExample) { + using namespace example; + Graph nfg = createNonlinearFactorGraph(); + Values c1 = createValues(); + EXPECT(equalsObj(nfg)); + EXPECT(equalsXML(nfg)); + + EXPECT(equalsObj(c1)); + EXPECT(equalsXML(c1)); } /* ************************************************************************* */ @@ -345,7 +473,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Range, "gtsam::planarSLAM::Rang BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); - /* ************************************************************************* */ TEST (Serialization, planar_system) { using namespace planarSLAM;