12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Serialized more classes in gtsam and gtsam_unstable

release/4.3a0
Alex Cunningham 2013-06-19 17:50:07 +00:00
parent 23de91d44d
commit 45b5389f8a
4 changed files with 65 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam.h
View File

@ -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<POSE, POINT, ROTATION>
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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
@ -2118,6 +2129,9 @@ template<POSE, POINT, ROTATION>
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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> 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<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
@ -2160,6 +2177,9 @@ template<CALIBRATION = {gtsam::Cal3_S2}>
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<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;

15
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -26,6 +26,9 @@ protected:
GaussianFactor::shared_ptr factor_;
boost::optional<Values> linearizationPoint_;
/** Default constructor - necessary for serialization */
LinearContainerFactor() {}
/** direct copy constructor */
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
const boost::optional<Values>& 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(factor_);
ar & BOOST_SERIALIZATION_NVP(linearizationPoint_);
}
}; // \class LinearContainerFactor
} // \namespace gtsam

14
gtsam_unstable/gtsam_unstable.h
View File

@ -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 <gtsam_unstable/geometry/Pose3Upright.h>
@ -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 <gtsam_unstable/geometry/BearingS2.h>
@ -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<gtsam::Point2>
@ -288,6 +294,8 @@ class SimPolygon2D {
template<T = {gtsam::PoseRTV}>
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<T = {gtsam::PoseRTV}>
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<POSE, POINT>
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<gtsam::PoseRTV, gtsam::PoseRTV> 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 <gtsam_unstable/dynamics/IMUFactor.h>

46
matlab/gtsam_tests/testGraphValuesSerialization.m
View File

@ -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));
CHECK('graphds.equals(graph, 1e-9)', graphds.equals(graph, 1e-9));