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

Add bearing and range factor tests

release/4.3a0
Frank Dellaert 2018-12-30 18:35:33 -05:00
parent fe1daec086
commit 63acd1a50c
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
matlab/gtsam_tests/testSerialization.m
View File

@ -52,14 +52,26 @@ 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
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 % Range Factors
rNoise = noiseModel.Diagonal.Sigmas([0.2]);
graph.add(RangeFactorPosePoint2(i1, j1, sqrt(4+4), rNoise));
graph.add(RangeFactorPosePoint2(i2, j1, 2, rNoise));
graph.add(RangeFactorPosePoint2(i3, j2, 2, rNoise));
% Bearing Factors
degrees = pi/180; degrees = pi/180;
bNoise = noiseModel.Diagonal.Sigmas([0.1]);
graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise));
graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise));
graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise));
% BearingRange Factors
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); 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(i2, j1, Rot2(90*degrees), 2, brNoise));