diff --git a/gtsam.h b/gtsam.h index 33149c344..dcef6e23d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1787,6 +1787,8 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, const gtsam::Ordering& ordering); + // enabling serialization functionality + void serializable() const; }; // \class LinearContainerFactor // Summarization functionality @@ -2073,6 +2075,9 @@ virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; gtsam::noiseModel::Base* get_noiseModel() const; + + // enabling serialization functionality + void serialize() const; }; @@ -2083,6 +2088,9 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { NonlinearEquality(size_t j, const T& feasible); // Constructor - allows inexact evaluation NonlinearEquality(size_t j, const T& feasible, double error_gain); + + // enabling serialization functionality + void serialize() const; }; @@ -2108,6 +2116,9 @@ template virtual class BearingFactor : gtsam::NonlinearFactor { BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); gtsam::noiseModel::Base* get_noiseModel() const; + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::BearingFactor BearingFactor2D; @@ -2118,6 +2129,9 @@ template virtual class BearingRangeFactor : gtsam::NonlinearFactor { BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); gtsam::noiseModel::Base* get_noiseModel() const; + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::BearingRangeFactor BearingRangeFactor2D; @@ -2142,6 +2156,9 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor { bool verboseCheirality() const; bool throwCheirality() const; gtsam::noiseModel::Base* get_noiseModel() const; + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; @@ -2160,6 +2177,9 @@ template virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor { GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); gtsam::Point2 measured() const; + + // enabling serialization functionality + void serialize() const; }; @@ -2171,6 +2191,9 @@ virtual class GenericStereoFactor : gtsam::NonlinearFactor { gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const; gtsam::noiseModel::Base* get_noiseModel() const; + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::GenericStereoFactor GenericStereoFactor3D; diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index b5b9729d1..cc87e278a 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -26,6 +26,9 @@ protected: GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; + /** Default constructor - necessary for serialization */ + LinearContainerFactor() {} + /** direct copy constructor */ LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); @@ -148,6 +151,18 @@ protected: void rekeyFactor(const Ordering& ordering); void initializeLinearizationPoint(const Values& linearizationPoint); +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(factor_); + ar & BOOST_SERIALIZATION_NVP(linearizationPoint_); + } + }; // \class LinearContainerFactor } // \namespace gtsam diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 27dd8ba1c..c4feeccef 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -86,6 +86,8 @@ virtual class PoseRTV : gtsam::Value { Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const; gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const; Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const; + + void serializable() const; // enabling serialization functionality }; #include @@ -122,6 +124,8 @@ virtual class Pose3Upright : gtsam::Value { static gtsam::Pose3Upright Expmap(Vector xi); static Vector Logmap(const gtsam::Pose3Upright& p); + + void serializable() const; // enabling serialization functionality }; // \class Pose3Upright #include @@ -142,6 +146,8 @@ virtual class BearingS2 : gtsam::Value { size_t dim() const; gtsam::BearingS2 retract(Vector v) const; Vector localCoordinates(const gtsam::BearingS2& p2) const; + + void serializable() const; // enabling serialization functionality }; // std::vector @@ -288,6 +294,8 @@ class SimPolygon2D { template virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + + void serializable() const; // enabling serialization functionality }; @@ -295,6 +303,8 @@ virtual class PriorFactor : gtsam::NonlinearFactor { template virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + + void serializable() const; // enabling serialization functionality }; @@ -302,6 +312,8 @@ virtual class BetweenFactor : gtsam::NonlinearFactor { template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + + void serializable() const; // enabling serialization functionality }; typedef gtsam::RangeFactor RangeFactorRTV; @@ -314,6 +326,8 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { NonlinearEquality(size_t j, const T& feasible); // Constructor - allows inexact evaluation NonlinearEquality(size_t j, const T& feasible, double error_gain); + + void serializable() const; // enabling serialization functionality }; #include diff --git a/matlab/gtsam_tests/testGraphValuesSerialization.m b/matlab/gtsam_tests/testGraphValuesSerialization.m index 999335bcd..9f49328cd 100644 --- a/matlab/gtsam_tests/testGraphValuesSerialization.m +++ b/matlab/gtsam_tests/testGraphValuesSerialization.m @@ -47,44 +47,24 @@ CHECK('valuesds.equals(values, 1e-9)', valuesds.equals(values, 1e-9)); %% Create graph and factors and serialize graph = NonlinearFactorGraph; -% Prior factor - FAIL: unregistered class +% Prior factor priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph -% % Between Factors - FAIL: unregistered class -% odometry = Pose2(2.0, 0.0, 0.0); -% odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -% graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); -% graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); +% Between Factors - FAIL: unregistered class +odometry = Pose2(2.0, 0.0, 0.0); +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]); -% graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); -% graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); -% graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); +% BearingRange Factors - FAIL: unregistered class +degrees = pi/180; +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); +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 -% serialized_values = serializeValues(values); % returns a string -% deserializedValues = deserializeValues(serialized_values); % returns a new values -% CHECK('values.equals(deserializedValues)',values.equals(deserializedValues,1e-9)); -% -% CHECK('serializeValuesToFile(values, values.dat)', serializeValuesToFile(values, 'values.dat')); -% deserializedValuesFile = deserializeValuesFromFile('values.dat'); % returns a new values -% CHECK('values.equals(deserializedValuesFile)',values.equals(deserializedValuesFile,1e-9)); -% -% % FAIL: unregistered class - derived class not registered or exported -% serialized_graph = serializeGraph(graph); % returns a string -% deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph -% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); -% -% CHECK('serializeGraphToFile(graph, graph.dat)', serializeGraphToFile(graph, 'graph.dat')); -% deserializedGraphFile = deserializeGraphFromFile('graph.dat'); % returns a new graph -% CHECK('graph.equals(deserializedGraphFile)',graph.equals(deserializedGraphFile,1e-9)); \ No newline at end of file +CHECK('graphds.equals(graph, 1e-9)', graphds.equals(graph, 1e-9)); \ No newline at end of file