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

Rearranged serialization test to use new interface, added serialize() flags to Point2 and Values. Serialzing values fails - now to add export commands

release/4.3a0
Alex Cunningham 2013-06-19 17:50:01 +00:00
parent 14c5f205dd
commit b5b1eac597
2 changed files with 69 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam.h
View File

@ -267,6 +267,9 @@ virtual class Point2 : gtsam::Value {
Vector vector() const; Vector vector() const;
double dist(const gtsam::Point2& p2) const; double dist(const gtsam::Point2& p2) const;
double norm() const; double norm() const;
// enabling serialization functionality
void serialize() const;
}; };
virtual class StereoPoint2 : gtsam::Value { virtual class StereoPoint2 : gtsam::Value {
@ -1555,29 +1558,32 @@ class Values {
size_t size() const; size_t size() const;
bool empty() const; bool empty() const;
void clear(); void clear();
size_t dim() const; size_t dim() const;
void print(string s) const; void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const; bool equals(const gtsam::Values& other, double tol) const;
void insert(size_t j, const gtsam::Value& value); void insert(size_t j, const gtsam::Value& value);
void insert(const gtsam::Values& values); void insert(const gtsam::Values& values);
void update(size_t j, const gtsam::Value& val); void update(size_t j, const gtsam::Value& val);
void update(const gtsam::Values& values); void update(const gtsam::Values& values);
void erase(size_t j); void erase(size_t j);
void swap(gtsam::Values& values); void swap(gtsam::Values& values);
bool exists(size_t j) const; bool exists(size_t j) const;
gtsam::Value at(size_t j) const; gtsam::Value at(size_t j) const;
gtsam::KeyList keys() const; gtsam::KeyList keys() const;
gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const; gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const;
gtsam::Ordering* orderingArbitrary(size_t firstVar) const; gtsam::Ordering* orderingArbitrary(size_t firstVar) const;
gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const; gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const;
gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const; gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const;
void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const; void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const;
// enabling serialization functionality
void serialize() const;
}; };
// Actually a FastList<Key> // Actually a FastList<Key>

81
matlab/gtsam_tests/testGraphValuesSerialization.m
View File

@ -6,7 +6,7 @@
% %
% See LICENSE for the license information % See LICENSE for the license information
% %
% @brief Simple robotics example using the pre-built planar SLAM domain % @brief Checks for serialization using basic string interface
% @author Alex Cunningham % @author Alex Cunningham
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -17,7 +17,34 @@ import gtsam.*
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2); j1 = symbol('l',1); j2 = symbol('l',2);
%% Create graph and factors %% Create values and verify string serialization
pose1=Pose2(0.5, 0.0, 0.2);
pose2=Pose2(2.3, 0.1,-0.2);
pose3=Pose2(4.1, 0.1, 0.1);
landmark1=Point2(1.8, 2.1);
landmark2=Point2(4.1, 1.8);
serialized_pose1 = pose1.string_serialize();
pose1ds = Pose2.string_deserialize(serialized_pose1);
CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9));
serialized_landmark1 = landmark1.string_serialize();
landmark1ds = Point2.string_deserialize(serialized_landmark1);
CHECK('landmark1ds.equals(landmark1, 1e-9)', landmark1ds.equals(landmark1, 1e-9));
%% Create and serialize Values
values = Values;
values.insert(i1, pose1);
values.insert(i2, pose2);
values.insert(i3, pose3);
values.insert(j1, landmark1);
values.insert(j2, landmark2);
serialized_values = values.string_serialize();
valuesds = Values.string_deserialize(serialized_values);
CHECK('valuesds.equals(values, 1e-9)', valuesds.equals(values, 1e-9));
%% Create graph and factors and serialize
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
% Prior factor - FAIL: unregistered class % Prior factor - FAIL: unregistered class
@ -25,36 +52,30 @@ priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
% Between Factors - FAIL: unregistered class % % Between Factors - FAIL: unregistered class
odometry = Pose2(2.0, 0.0, 0.0); % odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); % graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, 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 %% Old interface
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));
%% Create Values
values = Values;
values.insert(i1, Pose2(0.5, 0.0, 0.2));
values.insert(i2, Pose2(2.3, 0.1,-0.2));
values.insert(i3, Pose2(4.1, 0.1, 0.1));
values.insert(j1, Point2(1.8, 2.1));
values.insert(j2, Point2(4.1, 1.8));
%% 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));
% %% 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 % % FAIL: unregistered class - derived class not registered or exported
% serialized_graph = serializeGraph(graph); % returns a string % serialized_graph = serializeGraph(graph); % returns a string
% deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph % deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph