2016-06-11 08:48:42 +08:00
|
|
|
% Create graph container and add factors to it
|
|
|
|
|
graph = NonlinearFactorGraph;
|
|
|
|
|
|
|
|
|
|
% Create keys for variables
|
|
|
|
|
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
|
|
|
|
|
j1 = symbol('l',1); j2 = symbol('l',2);
|
|
|
|
|
|
|
|
|
|
% Add prior
|
|
|
|
|
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
|
|
|
|
|
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
|
|
|
|
|
% add directly to graph
|
|
|
|
|
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));
|
|
|
|
|
|
|
|
|
|
% Add odometry
|
|
|
|
|
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));
|
|
|
|
|
|
|
|
|
|
% Add bearing/range measurement factors
|
|
|
|
|
degrees = pi/180;
|
2016-06-16 02:26:28 +08:00
|
|
|
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
|
|
|
|
|
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));
|
|
|
|
|
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
|
|
|
|
|
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
|
