diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index 9f49328cd..baacb198b 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -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]); 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); 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 +% 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; +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]); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));