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

Moved serialization tests over to a single test scenario - factors don't appear to work at the moment

release/4.3a0
Alex Cunningham 2013-06-12 20:01:59 +00:00
parent e69af84c36
commit 527ea5e511
4 changed files with 59 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
matlab/gtsam_tests/testGraphValuesSerialization.m Normal file
View File

@ -0,0 +1,56 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Simple robotics example using the pre-built planar SLAM domain
% @author Alex Cunningham
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);
%% Create graph and factors
graph = NonlinearFactorGraph;
% Prior factor - FAIL: unregistered class
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));
% 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));
%% 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));
serialized_graph = serializeGraph(graph); % returns a string
deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph
CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9));

10
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -67,13 +67,3 @@ CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
point_1 = result.at(symbol('l',1)); point_1 = result.at(symbol('l',1));
marginals.marginalCovariance(symbol('l',1)); marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));
%% Check that serialization works properly
serialized_values = serializeValues(initialEstimate);
deserializedValues = deserializeValues(serialized_values);
CHECK('initialEstimate.equals(deserializedValues)',initialEstimate.equals(deserializedValues,1e-9));
% FAILS: unregistered class?
% serialized_graph = serializeGraph(graph);
% deserializedGraph = deserializeGraph(serialized_graph);
% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9));

10
matlab/gtsam_tests/testPose2SLAMExample.m
View File

@ -60,13 +60,3 @@ P = marginals.marginalCovariance(1);
pose_1 = result.at(1); pose_1 = result.at(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
%% Check that serialization works properly
serialized_values = serializeValues(initialEstimate);
deserializedValues = deserializeValues(serialized_values);
CHECK('initialEstimate.equals(deserializedValues)',initialEstimate.equals(deserializedValues,1e-9));
% FAILS: unregistered class?
% serialized_graph = serializeGraph(graph);
% deserializedGraph = deserializeGraph(serialized_graph);
% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9));

3
matlab/gtsam_tests/test_gtsam.m
View File

@ -30,5 +30,8 @@ testStereoVOExample
display 'Starting: testVisualISAMExample' display 'Starting: testVisualISAMExample'
testVisualISAMExample testVisualISAMExample
display 'Starting: testGraphValuesSerialization'
testVisualISAMExample
% end of tests % end of tests
display 'Tests complete!' display 'Tests complete!'