From da012682a564594f9b5b46b13f823181bb766f11 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 3 Jan 2025 12:41:28 +0200 Subject: [PATCH] Triangulation example with an outlier --- test/testRobustParametic2D.jl | 44 +++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 test/testRobustParametic2D.jl diff --git a/test/testRobustParametic2D.jl b/test/testRobustParametic2D.jl new file mode 100644 index 00000000..2bb2670d --- /dev/null +++ b/test/testRobustParametic2D.jl @@ -0,0 +1,44 @@ +using RoME +using Test + +@testset "Triangulation test in 2D, 4 beacons with outlier" begin +# landmark prior noise +lmp_noise = diagm([0.01;0.01].^2) + +# create a new factor graph and set to not auto initialize +fg = initfg() +fg.solverParams.graphinit = false + +# add 3 landmarks variables for inliers points (TranslationGroup(2)) with prior mesurements for each +addVariable!(fg, :l1, Point2) +addVariable!(fg, :l2, Point2) +addVariable!(fg, :l3, Point2) + +addFactor!(fg, [:l1], PriorPoint2(MvNormal([10.0;1.0],lmp_noise))) +addFactor!(fg, [:l2], PriorPoint2(MvNormal([10.0+sqrt(3)/2;-0.5],lmp_noise))) +addFactor!(fg, [:l3], PriorPoint2(MvNormal([10.0-sqrt(3)/2;-0.5],lmp_noise))) + +# Add a 2D Pose (SE2) with measurements to landmarks for pose to end up at (10, 0, 0) +addVariable!(fg, :x1, Pose2) +addFactor!(fg, [:x1, :l1], Pose2Point2Bearing(Normal(pi/2,0.05))) +addFactor!(fg, [:x1, :l2], Pose2Point2Bearing(Normal(-pi/6,0.05))) +addFactor!(fg, [:x1, :l3], Pose2Point2Bearing(Normal(-pi+pi/6,0.05))) + +#solve the graph - calls solve_RLM which uses Manopt.LevenbergMarquardt! +IIF.solveGraphParametric!(fg) +# get the result of the pose +x1_in = getVal(fg, :x1; solveKey=:parametric)[1] + +@test isapprox(x1_in, ArrayPartition([10.0, 0], [1.0 0; 0 1.0])) + +# Add the outlier landmark (l4) and measurements +addVariable!(fg, :l4, Point2) +addFactor!(fg, [:l4], PriorPoint2(MvNormal([10.5;1.0],lmp_noise))) +addFactor!(fg, [:x1, :l4], Pose2Point2Bearing(Normal(pi/2,0.05))) + +IIF.solveGraphParametric!(fg) +x1_out = getVal(fg, :x1; solveKey=:parametric)[1] +# the outlier scews the result to +# ([10.163, -0.068], [0.9970 0.077; -0.077 0.997]) + +end \ No newline at end of file