From 23de91d44dc88f563dd1c713cdba5d238381ca7e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 19 Jun 2013 17:50:05 +0000 Subject: [PATCH] serialization works for graph and PriorFactor. Added second flag for serialization: can add "void serializable()" or "void serialize()" to allow for either just exporting (necessary if no default constructor), or implementing the full serialization functions --- gtsam.h | 90 +++++++++++++++++++ .../testGraphValuesSerialization.m | 6 +- wrap/Class.cpp | 6 +- wrap/Class.h | 5 +- wrap/Module.cpp | 7 ++ wrap/tests/expected/geometry_wrapper.cpp | 1 + wrap/tests/geometry.h | 2 + wrap/tests/testWrap.cpp | 5 ++ 8 files changed, 116 insertions(+), 6 deletions(-) diff --git a/gtsam.h b/gtsam.h index 081eccb53..33149c344 100644 --- a/gtsam.h +++ b/gtsam.h @@ -200,6 +200,9 @@ virtual class LieVector : gtsam::Value { // Lie group static gtsam::LieVector Expmap(Vector v); static Vector Logmap(const gtsam::LieVector& p); + + // enabling serialization functionality + void serialize() const; }; #include @@ -229,6 +232,9 @@ virtual class LieMatrix : gtsam::Value { // Lie group static gtsam::LieMatrix Expmap(Vector v); static Vector Logmap(const gtsam::LieMatrix& p); + + // enabling serialization functionality + void serialize() const; }; //************************************************************************* @@ -299,6 +305,9 @@ virtual class StereoPoint2 : gtsam::Value { // Standard Interface Vector vector() const; + + // enabling serialization functionality + void serialize() const; }; virtual class Point3 : gtsam::Value { @@ -332,6 +341,9 @@ virtual class Point3 : gtsam::Value { double x() const; double y() const; double z() const; + + // enabling serialization functionality + void serialize() const; }; virtual class Rot2 : gtsam::Value { @@ -374,6 +386,9 @@ virtual class Rot2 : gtsam::Value { double c() const; double s() const; Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; }; virtual class Rot3 : gtsam::Value { @@ -427,6 +442,9 @@ virtual class Rot3 : gtsam::Value { double yaw() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly Vector quaternion() const; + + // enabling serialization functionality + void serialize() const; }; virtual class Pose2 : gtsam::Value { @@ -524,6 +542,9 @@ virtual class Pose3 : gtsam::Value { gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() double range(const gtsam::Point3& point); double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; }; virtual class Cal3_S2 : gtsam::Value { @@ -557,6 +578,9 @@ virtual class Cal3_S2 : gtsam::Value { Vector vector() const; Matrix matrix() const; Matrix matrix_inverse() const; + + // enabling serialization functionality + void serialize() const; }; #include @@ -591,6 +615,9 @@ virtual class Cal3DS2 : gtsam::Value { Vector vector() const; Vector k() const; //Matrix K() const; //FIXME: Uppercase + + // enabling serialization functionality + void serialize() const; }; class Cal3_S2Stereo { @@ -640,6 +667,9 @@ virtual class CalibratedCamera : gtsam::Value { // Standard Interface gtsam::Pose3 pose() const; double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods + + // enabling serialization functionality + void serialize() const; }; virtual class SimpleCamera : gtsam::Value { @@ -675,6 +705,9 @@ virtual class SimpleCamera : gtsam::Value { gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; }; template @@ -711,6 +744,9 @@ virtual class PinholeCamera : gtsam::Value { gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; }; //************************************************************************* @@ -994,6 +1030,9 @@ virtual class Gaussian : gtsam::noiseModel::Base { Matrix R() const; bool equals(gtsam::noiseModel::Base& expected, double tol); void print(string s) const; + + // enabling serialization functionality + void serializable() const; }; virtual class Diagonal : gtsam::noiseModel::Gaussian { @@ -1002,6 +1041,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); Matrix R() const; void print(string s) const; + + // enabling serialization functionality + void serializable() const; }; virtual class Constrained : gtsam::noiseModel::Diagonal { @@ -1016,6 +1058,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { static gtsam::noiseModel::Constrained* All(size_t dim, double mu); gtsam::noiseModel::Constrained* unit() const; + + // enabling serialization functionality + void serializable() const; }; virtual class Isotropic : gtsam::noiseModel::Diagonal { @@ -1023,11 +1068,17 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); void print(string s) const; + + // enabling serialization functionality + void serializable() const; }; virtual class Unit : gtsam::noiseModel::Isotropic { static gtsam::noiseModel::Unit* Create(size_t dim); void print(string s) const; + + // enabling serialization functionality + void serializable() const; }; namespace mEstimator { @@ -1038,24 +1089,36 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { Null(); void print(string s) const; static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { Fair(double c); void print(string s) const; static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { Huber(double k); void print(string s) const; static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { Tukey(double k); void print(string s) const; static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; }; }///\namespace mEstimator @@ -1064,6 +1127,9 @@ virtual class Robust : gtsam::noiseModel::Base { Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); void print(string s) const; + + // enabling serialization functionality + void serializable() const; }; }///\namespace noiseModel @@ -1118,6 +1184,9 @@ class VectorValues { bool hasSameStructure(const gtsam::VectorValues& other) const; double dot(const gtsam::VectorValues& V) const; double norm() const; + + // enabling serialization functionality + void serialize() const; }; class GaussianConditional { @@ -1140,6 +1209,9 @@ class GaussianConditional { void solveInPlace(gtsam::VectorValues& x) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const; void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + + // enabling serialization functionality + void serialize() const; }; class GaussianDensity { @@ -1250,6 +1322,9 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void assertInvariants() const; //gtsam::SharedDiagonal& get_model(); + + // enabling serialization functionality + void serialize() const; }; virtual class HessianFactor : gtsam::GaussianFactor { @@ -1283,6 +1358,9 @@ virtual class HessianFactor : gtsam::GaussianFactor { void partialCholesky(size_t nrFrontals); gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals); void assertInvariants() const; + + // enabling serialization functionality + void serialize() const; }; class GaussianFactorGraph { @@ -1329,6 +1407,9 @@ class GaussianFactorGraph { pair jacobian() const; Matrix augmentedHessian() const; pair hessian() const; + + // enabling serialization functionality + void serialize() const; }; //Non-Class functions in GaussianFactorGraph.h @@ -1508,6 +1589,9 @@ class Ordering { void push_back(size_t key); void permuteInPlace(const gtsam::Permutation& permutation); void permuteInPlace(const gtsam::Permutation& selector, const gtsam::Permutation& permutation); + + // enabling serialization functionality + void serialize() const; }; class NonlinearFactorGraph { @@ -1535,6 +1619,9 @@ class NonlinearFactorGraph { const gtsam::Ordering& ordering) const; gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const; gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; }; virtual class NonlinearFactor { @@ -1974,6 +2061,9 @@ virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; gtsam::noiseModel::Base* get_noiseModel() const; + + // enabling serialization functionality + void serialize() const; }; diff --git a/matlab/gtsam_tests/testGraphValuesSerialization.m b/matlab/gtsam_tests/testGraphValuesSerialization.m index 23401f370..999335bcd 100644 --- a/matlab/gtsam_tests/testGraphValuesSerialization.m +++ b/matlab/gtsam_tests/testGraphValuesSerialization.m @@ -57,7 +57,7 @@ graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph % odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); % graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); -% + % % BearingRange Factors - FAIL: unregistered class % degrees = pi/180; % brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); @@ -65,6 +65,10 @@ graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph % graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); % graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); +serialized_graph = graph.string_serialize(); +graphds = NonlinearFactorGraph.string_deserialize(serialized_graph); +CHECK('graphds.equals(graph, 1e-9)', graphds.equals(graph, 1e-9)); + %% Old interface % %% Check that serialization works properly diff --git a/wrap/Class.cpp b/wrap/Class.cpp index c384a5377..15c4a742c 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -111,7 +111,7 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, proxyFile.oss << "\n"; wrapperFile.oss << "\n"; } - if (isSerializable) + if (hasSerialization) serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); proxyFile.oss << " end\n"; @@ -125,7 +125,7 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, proxyFile.oss << "\n"; wrapperFile.oss << "\n"; } - if (isSerializable) + if (hasSerialization) deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); proxyFile.oss << " end\n"; @@ -393,7 +393,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const { } } - if (isSerializable) { + if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n"; diff --git a/wrap/Class.h b/wrap/Class.h index efbcce26c..f5267cee2 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -37,14 +37,15 @@ struct Class { typedef std::map StaticMethods; /// Constructor creates an empty class - Class(bool verbose=true) : isVirtual(false), isSerializable(false), verbose_(verbose) {} + Class(bool verbose=true) : isVirtual(false), isSerializable(false), hasSerialization(false), verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor std::string name; ///< Class name std::vector templateArgs; ///< Template arguments std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] bool isVirtual; ///< Whether the class is part of a virtual inheritance chain - bool isSerializable; ///< Whether we can use boost.serialization to serialize the class + bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports + bool hasSerialization; ///< Whether we should create the serialization functions std::vector qualifiedParent; ///< The *single* parent - the last string is the parent class name, preceededing elements are a namespace stack Methods methods; ///< Class methods StaticMethods static_methods; ///< Static methods diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 3f16fbf1b..a31f57e79 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -380,9 +380,16 @@ void Module::parseMarkup(const std::string& data) { // Post-process classes for serialization markers BOOST_FOREACH(Class& cls, classes) { + Class::Methods::iterator serializable_it = cls.methods.find("serializable"); + if (serializable_it != cls.methods.end()) { + cls.isSerializable = true; + cls.methods.erase(serializable_it); + } + Class::Methods::iterator serialize_it = cls.methods.find("serialize"); if (serialize_it != cls.methods.end()) { cls.isSerializable = true; + cls.hasSerialization= true; cls.methods.erase(serialize_it); } } diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 1dfea33d4..e00004d57 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -9,6 +9,7 @@ #include +BOOST_CLASS_EXPORT_GUID(Point2, "Point2"); BOOST_CLASS_EXPORT_GUID(Point3, "Point3"); typedef std::set*> Collector_Point2; diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 6c6ed28ad..bdced45ed 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -13,6 +13,8 @@ class Point2 { void argChar(char a) const; void argUChar(unsigned char a) const; VectorNotEigen vectorConfusion(); + + void serializable() const; // Sets flag and creates export, but does not make serialization functions }; class Point3 { diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index f993867e5..461f00405 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -219,6 +219,10 @@ TEST( wrap, parse_geometry ) { EXPECT_LONGS_EQUAL(0, cls.static_methods.size()); EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + + // check serialization flag + EXPECT(cls.isSerializable); + EXPECT(!cls.hasSerialization); } // check second class, Point3 @@ -254,6 +258,7 @@ TEST( wrap, parse_geometry ) { // check serialization flag EXPECT(cls.isSerializable); + EXPECT(cls.hasSerialization); } // Test class is the third one