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

Works, but is really a test, not an example

release/4.3a0
dellaert 2014-05-05 11:26:16 -04:00
parent f5e0a7f2b1
commit 9409357fe7
1 changed files with 36 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
matlab/unstable_examples/TSAMFactorsExample.m
View File

@ -4,41 +4,59 @@
import gtsam.*;
% noise models
noisePrior = noiseModel.Diagonal.Sigmas([0; 0; 0]);
noisePrior = noiseModel.Diagonal.Sigmas([100; 100; 100]);
noiseDelta = noiseModel.Isotropic.Sigma(2, 0.1);
noiseOdom = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]);
% Example is 1, landmark, 2 poses 10 and 20, 2 base nodes 100 and 200
% Example is 2 landmarks 1 and 2, 2 poses 10 and 20, 2 base nodes 100 and 200
% l1 l2
% + +
% - b - p - l - p - b
% +---+-------+---+
% - b - p - p - b
% +---+---+---+
% True values
b1 = Pose2(0,0,0);
b2 = Pose2(2,0,0);
l1 = Point2(0,1);
l2 = Point2(2,1);
% Create a graph
graph = NonlinearFactorGraph
graph = NonlinearFactorGraph;
origin = Pose2;
graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior))
graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior))
graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior))
graph.add(DeltaFactor(10, 1, Point2(1,0), noiseDelta))
graph.add(DeltaFactor(20, 1, Point2(-1,0), noiseDelta))
graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior))
graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta))
graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta))
graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta))
graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta))
graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom))
graph
% Initial values
initial = Values;
initial.insert(100,origin);
initial.insert(10,origin);
initial.insert(1,Point2(2,0));
initial.insert(1,l1);
initial.insert(2,l2);
initial.insert(20,origin);
initial.insert(200,origin);
graph.error(initial)
graph.at(0).error(initial)
graph.at(1).error(initial)
graph.at(2).error(initial)
graph.at(3).error(initial)
% optimize
params = LevenbergMarquardtParams;
params.setVerbosity('ERROR');
% params.setVerbosity('ERROR');
optimizer = LevenbergMarquardtOptimizer(graph, initial, params);
result = optimizer.optimize()
result = optimizer.optimize();
% Check result
CHECK('error',result.at(100).equals(b1,1e-5))
CHECK('error',result.at(10).equals(origin,1e-5))
CHECK('error',result.at(1).equals(Point2(0,1),1e-5))
CHECK('error',result.at(2).equals(Point2(0,1),1e-5))
CHECK('error',result.at(20).equals(origin,1e-5))
CHECK('error',result.at(200).equals(b2,1e-5))
% Check error
CHECK('error',abs(graph.error(result))<1e-9)
for i=0:7
CHECK('error',abs(graph.at(i).error(result))<1e-9)
end