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

Fixed gtsam_test (except serialize)

release/4.3a0
dellaert 2014-11-13 23:59:51 +01:00
parent d9e8ec400b
commit 4fb83694a7
9 changed files with 24 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
matlab/gtsam_tests/testLocalizationExample.m
View File

@ -29,7 +29,7 @@ groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
for i=1:3
graph.add(PriorFactorPose2(i, groundTruth.at(i), model));
graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
end
%% Initialize to noisy points
@ -46,7 +46,7 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
P={};
for i=1:result.size()
pose_i = result.at(i);
CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4));
pose_i = result.atPose2(i);
CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4));
P{i}=marginals.marginalCovariance(i);
end

2
matlab/gtsam_tests/testOdometryExample.m
View File

@ -39,5 +39,5 @@ marginals = Marginals(graph, result);
marginals.marginalCovariance(1);
%% Check first pose equality
pose_1 = result.at(1);
pose_1 = result.atPose2(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

4
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -60,10 +60,10 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
%% Check first pose and point equality
pose_1 = result.at(symbol('x',1));
pose_1 = result.atPose2(symbol('x',1));
marginals.marginalCovariance(symbol('x',1));
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
point_1 = result.at(symbol('l',1));
point_1 = result.atPoint2(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));

2
matlab/gtsam_tests/testPose2SLAMExample.m
View File

@ -57,6 +57,6 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
P = marginals.marginalCovariance(1);
pose_1 = result.at(1);
pose_1 = result.atPose2(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

16
matlab/gtsam_tests/testPose3SLAMExample.m
View File

@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0);
p1 = hexagon.at(1);
p0 = hexagon.atPose3(0);
p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values;
s = 0.10;
initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% optimize
optimizer = LevenbergMarquardtOptimizer(fg, initial);
result = optimizer.optimizeSafely;
pose_1 = result.at(1);
pose_1 = result.atPose3(1);
CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4));

4
matlab/gtsam_tests/testSFMExample.m
View File

@ -63,11 +63,11 @@ marginals.marginalCovariance(symbol('x',1));
%% Check optimized results, should be equal to ground truth
for i=1:size(truth.cameras,2)
pose_i = result.at(symbol('x',i));
pose_i = result.atPose3(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.at(symbol('p',j));
point_j = result.atPoint3(symbol('p',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

4
matlab/gtsam_tests/testStereoVOExample.m
View File

@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize();
%% check equality for the first pose and point
pose_x1 = result.at(x1);
pose_x1 = result.atPose3(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
point_l1 = result.at(l1);
point_l1 = result.atPoint3(l1);
CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));

4
matlab/gtsam_tests/testVisualISAMExample.m
View File

@ -45,11 +45,11 @@ for frame_i=3:options.nrCameras
end
for i=1:size(truth.cameras,2)
pose_i = result.at(symbol('x',i));
pose_i = result.atPose3(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.at(symbol('l',j));
point_j = result.atPoint3(symbol('l',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

6
matlab/gtsam_tests/test_gtsam.m
View File

@ -30,11 +30,11 @@ testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
display 'Starting: testSerialization'
testSerialization
display 'Starting: testUtilities'
testUtilities
display 'Starting: testSerialization'
testSerialization
% end of tests
display 'Tests complete!'