@@ -11,7 +11,7 @@ import Rotations as _Rot
11
11
12
12
@defVariable TranslationGroup2 TranslationGroup (2 ) @SVector [0.0 , 0.0 ]
13
13
14
- @defVariable SpecialEuclidean2 SpecialEuclidean (2 ) ArrayPartition (@SVector ([0.0 ,0.0 ]), @SMatrix ([1.0 0.0 ; 0.0 1.0 ]))
14
+ @defVariable SpecialEuclidean2 SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ) ArrayPartition (@SVector ([0.0 ,0.0 ]), @SMatrix ([1.0 0.0 ; 0.0 1.0 ]))
15
15
# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0])
16
16
17
17
# #
@@ -20,7 +20,7 @@ import Rotations as _Rot
20
20
# #
21
21
22
22
M = getManifold (SpecialEuclidean2)
23
- @test M == SpecialEuclidean (2 )
23
+ @test M == SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () )
24
24
pT = getPointType (SpecialEuclidean2)
25
25
# @test pT == ArrayPartition{Float64,Tuple{Vector{Float64}, Matrix{Float64}}}
26
26
# @test pT == ArrayPartition{Tuple{MVector{2, Float64}, MMatrix{2, 2, Float64, 4}}}
@@ -38,8 +38,8 @@ v0 = addVariable!(fg, :x0, SpecialEuclidean2)
38
38
39
39
# mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
40
40
# mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal(Diagonal(abs2.([0.01, 0.01, 0.01]))))
41
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition ([0.0 ,0.0 ], [1.0 0.0 ; 0.0 1. ]), MvNormal (Diagonal (abs2 .([0.01 , 0.01 , 0.01 ]))))
42
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (SA[0.0 ,0.0 ], SA[1.0 0.0 ; 0.0 1. ]), MvNormal (Diagonal (abs2 .(SA[0.01 , 0.01 , 0.01 ]))))
41
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition ([0.0 ,0.0 ], [1.0 0.0 ; 0.0 1. ]), MvNormal (Diagonal (abs2 .([0.01 , 0.01 , 0.01 ]))))
42
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (SA[0.0 ,0.0 ], SA[1.0 0.0 ; 0.0 1. ]), MvNormal (Diagonal (abs2 .(SA[0.01 , 0.01 , 0.01 ]))))
43
43
p = addFactor! (fg, [:x0 ], mp)
44
44
45
45
@@ -54,7 +54,7 @@ vnd = getVariableSolverData(fg, :x0)
54
54
55
55
# #
56
56
v1 = addVariable! (fg, :x1 , SpecialEuclidean2)
57
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal (SA[1 ,2 ,pi / 4 ], SA[0.01 ,0.01 ,0.01 ]))
57
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal (SA[1 ,2 ,pi / 4 ], SA[0.01 ,0.01 ,0.01 ]))
58
58
f = addFactor! (fg, [:x0 , :x1 ], mf)
59
59
60
60
doautoinit! (fg, :x1 )
@@ -77,7 +77,7 @@ vnd = getVariableSolverData(fg, :x1)
77
77
@test all (is_point .(Ref (M), vnd. val))
78
78
79
79
v1 = addVariable! (fg, :x2 , SpecialEuclidean2)
80
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal (SA[1 ,2 ,pi / 4 ], SA[0.01 ,0.01 ,0.01 ]))
80
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal (SA[1 ,2 ,pi / 4 ], SA[0.01 ,0.01 ,0.01 ]))
81
81
f = addFactor! (fg, [:x1 , :x2 ], mf)
82
82
83
83
# #
@@ -140,7 +140,7 @@ struct ManifoldFactorSE2{T <: SamplableBelief} <: IIF.AbstractManifoldMinimize
140
140
end
141
141
142
142
ManifoldFactorSE2 () = ManifoldFactorSE2 (MvNormal (Diagonal ([1 ,1 ,1 ])))
143
- DFG. getManifold (:: ManifoldFactorSE2 ) = SpecialEuclidean (2 )
143
+ DFG. getManifold (:: ManifoldFactorSE2 ) = SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () )
144
144
145
145
IIF. selectFactorType (:: Type{<:SpecialEuclidean2} , :: Type{<:SpecialEuclidean2} ) = ManifoldFactorSE2
146
146
@@ -168,7 +168,7 @@ M = getManifold(SpecialEuclidean2)
168
168
fg = initfg ()
169
169
v0 = addVariable! (fg, :x0 , SpecialEuclidean2)
170
170
171
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([10.0 ,10.0 ]), Matrix ([- 1.0 0.0 ; 0.0 - 1.0 ])), MvNormal ([0.1 , 0.1 , 0.01 ]))
171
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([10.0 ,10.0 ]), Matrix ([- 1.0 0.0 ; 0.0 - 1.0 ])), MvNormal ([0.1 , 0.1 , 0.01 ]))
172
172
p = addFactor! (fg, [:x0 ], mp)
173
173
174
174
# #
@@ -177,16 +177,16 @@ for i in 0:5
177
177
psym = Symbol (" x$i " )
178
178
nsym = Symbol (" x$(i+ 1 ) " )
179
179
addVariable! (fg, nsym, SpecialEuclidean2)
180
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal ([10.0 ,0 ,pi / 3 ], [0.5 ,0.5 ,0.05 ]))
180
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal ([10.0 ,0 ,pi / 3 ], [0.5 ,0.5 ,0.05 ]))
181
181
f = addFactor! (fg, [psym;nsym], mf)
182
182
end
183
183
184
184
185
185
addVariable! (fg, :l1 , SpecialEuclidean2, tags= [:LANDMARK ;])
186
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal ([10.0 ,0 ,0 ], [0.1 ,0.1 ,0.01 ]))
186
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal ([10.0 ,0 ,0 ], [0.1 ,0.1 ,0.01 ]))
187
187
addFactor! (fg, [:x0 ; :l1 ], mf)
188
188
189
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal ([10.0 ,0 ,0 ], [0.1 ,0.1 ,0.01 ]))
189
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal ([10.0 ,0 ,0 ], [0.1 ,0.1 ,0.01 ]))
190
190
addFactor! (fg, [:x6 ; :l1 ], mf)
191
191
192
192
# #
@@ -225,7 +225,7 @@ getSolverParams(fg).useMsgLikelihoods = true
225
225
addVariable! (fg, :x0 , SpecialEuclidean2)
226
226
addVariable! (fg, :x1 , SpecialEuclidean2)
227
227
228
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([10.0 ,10.0 ]), Matrix ([- 1.0 0.0 ; 0.0 - 1.0 ])), MvNormal (diagm ([0.1 , 0.1 , 0.01 ]. ^ 2 )))
228
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([10.0 ,10.0 ]), Matrix ([- 1.0 0.0 ; 0.0 - 1.0 ])), MvNormal (diagm ([0.1 , 0.1 , 0.01 ]. ^ 2 )))
229
229
p = addFactor! (fg, [:x0 ], mp)
230
230
231
231
doautoinit! (fg,:x0 )
@@ -238,7 +238,7 @@ initAll!(fg)
238
238
239
239
pred, meas = approxDeconv (fg, :x0x1f1 )
240
240
241
- @test mmd (SpecialEuclidean (2 ), pred, meas) < 1e-1
241
+ @test mmd (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), pred, meas) < 1e-1
242
242
243
243
p_t = map (x-> x. x[1 ], pred)
244
244
m_t = map (x-> x. x[1 ], meas)
@@ -278,7 +278,7 @@ DFG.getManifold(::ManiPose2Point2) = TranslationGroup(2)
278
278
# define the conditional probability constraint
279
279
function (cfo:: CalcFactor{<:ManiPose2Point2} )(measX, p, q)
280
280
#
281
- M = SpecialEuclidean (2 )
281
+ M = SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () )
282
282
q_SE = ArrayPartition (q, identity_element (SpecialOrthogonal (2 ), p. x[2 ]))
283
283
284
284
X_se2 = log (M, identity_element (M, p), Manifolds. compose (M, inv (M, p), q_SE))
@@ -288,7 +288,7 @@ function (cfo::CalcFactor{<:ManiPose2Point2})(measX, p, q)
288
288
end
289
289
290
290
# #
291
- @testset " Test SpecialEuclidean(2)" begin
291
+ @testset " Test SpecialEuclidean(2; vectors=HybridTangentRepresentation() )" begin
292
292
# #
293
293
294
294
# Base.convert(::Type{<:Tuple}, M::TranslationGroup{Tuple{2},ℝ}) = (:Euclid, :Euclid)
@@ -299,7 +299,7 @@ fg = initfg()
299
299
300
300
v0 = addVariable! (fg, :x0 , SpecialEuclidean2)
301
301
302
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
302
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
303
303
p = addFactor! (fg, [:x0 ], mp)
304
304
305
305
# #
@@ -413,7 +413,7 @@ solveGraph!(fg; smtasks);
413
413
414
414
# #
415
415
416
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ],[1 0 0 ;0 1 0 ;0 0 1. ]))
416
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ],[1 0 0 ;0 1 0 ;0 0 1. ]))
417
417
f1 = addFactor! (fg, [:x0 ], mp, graphinit= false )
418
418
419
419
@test length (ls (fg, :x0 )) == 2
@@ -471,15 +471,15 @@ f0 = addFactor!(fg, [:x0], pthru, graphinit=false)
471
471
472
472
# # test the inference functions
473
473
addVariable! (fg, :x1 , SpecialEuclidean2)
474
- # mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
475
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
474
+ # mp = ManifoldPrior(SpecialEuclidean(2; vectors=HybridTangentRepresentation() ), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
475
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
476
476
f1 = addFactor! (fg, [:x1 ], mp, graphinit= false )
477
477
478
478
doautoinit! (fg, :x1 )
479
479
480
480
# # connect with relative and check calculation size on x0
481
481
482
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal ([1 ,2 ,pi / 4 ], [0.01 ,0.01 ,0.01 ]))
482
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal ([1 ,2 ,pi / 4 ], [0.01 ,0.01 ,0.01 ]))
483
483
f2 = addFactor! (fg, [:x0 , :x1 ], mf, graphinit= false )
484
484
485
485
# #
@@ -507,10 +507,10 @@ hmd = LevelSetGridNormal(img_, (x_,y_), 5.5, 0.1, N=120)
507
507
pthru = PartialPriorPassThrough (hmd, (1 ,2 ))
508
508
f0 = addFactor! (fg, [:x0 ], pthru, graphinit= false )
509
509
addVariable! (fg, :x1 , SpecialEuclidean2)
510
- # mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
511
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
510
+ # mp = ManifoldPrior(SpecialEuclidean(2; vectors=HybridTangentRepresentation() ), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
511
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
512
512
f1 = addFactor! (fg, [:x1 ], mp, graphinit= false )
513
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal ([1 ,2 ,pi / 4 ], [0.01 ,0.01 ,0.01 ]))
513
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal ([1 ,2 ,pi / 4 ], [0.01 ,0.01 ,0.01 ]))
514
514
f2 = addFactor! (fg, [:x0 , :x1 ], mf, graphinit= false )
515
515
516
516
# #
@@ -531,16 +531,16 @@ initAll!(fg)
531
531
end
532
532
533
533
534
- @testset " Test SpecialEuclidean(2) to TranslationGroup(2) multihypo" begin
534
+ @testset " Test SpecialEuclidean(2; vectors=HybridTangentRepresentation() ) to TranslationGroup(2) multihypo" begin
535
535
# #
536
536
537
537
fg = initfg ()
538
538
# fg.solverParams.attemptGradients=false
539
539
540
540
v0 = addVariable! (fg, :x0 , SpecialEuclidean2)
541
541
542
- # mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
543
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
542
+ # mp = ManifoldPrior(SpecialEuclidean(2; vectors=HybridTangentRepresentation() ), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
543
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
544
544
p = addFactor! (fg, [:x0 ], mp)
545
545
546
546
# #
@@ -552,7 +552,7 @@ f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
552
552
solveTree! (fg)
553
553
554
554
vnd = getVariableSolverData (fg, :x0 )
555
- @test isapprox (SpecialEuclidean (2 ) , mean (SpecialEuclidean (2 ), vnd. val), ArrayPartition ([0.0 ,0.0 ], [1.0 0 ; 0 1 ]), atol= 0.1 )
555
+ @test isapprox (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation ()) , mean (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), vnd. val), ArrayPartition ([0.0 ,0.0 ], [1.0 0 ; 0 1 ]), atol= 0.1 )
556
556
557
557
# FIXME I would expect close to 50% of particles to land on the correct place
558
558
# Currently software works so that 33% should land there so testing 20 for now
@@ -573,8 +573,8 @@ addVariable!(fg, :x0, SpecialEuclidean2)
573
573
addVariable! (fg, :x1a , TranslationGroup2)
574
574
addVariable! (fg, :x1b , TranslationGroup2)
575
575
576
- # mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([10, 10, 0.01]))
577
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal (zeros (3 ),diagm ([10 , 10 , 0.01 ])))
576
+ # mp = ManifoldPrior(SpecialEuclidean(2; vectors=HybridTangentRepresentation() ), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([10, 10, 0.01]))
577
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal (zeros (3 ),diagm ([10 , 10 , 0.01 ])))
578
578
p = addFactor! (fg, [:x0 ], mp)
579
579
mp = ManifoldPrior (TranslationGroup (2 ), [1. ,0 ], MvNormal ([0.01 , 0.01 ]))
580
580
p = addFactor! (fg, [:x1a ], mp)
@@ -595,44 +595,44 @@ pnts = getPoints(fg, :x0)
595
595
@error " Invalid multihypo test"
596
596
if false
597
597
# FIXME ManiPose2Point2 factor mean [1.,0] cannot go "backwards" from [0,0] to [-1,0] with covariance 0.01 -- wholly inconsistent test design
598
- @test 10 < sum (isapprox .(Ref (SpecialEuclidean (2 )), pnts, Ref (ArrayPartition ([- 1.0 ,0.0 ], [1.0 0 ; 0 1 ])), atol= 0.5 ))
599
- @test 10 < sum (isapprox .(Ref (SpecialEuclidean (2 )), pnts, Ref (ArrayPartition ([1.0 ,0.0 ], [1.0 0 ; 0 1 ])), atol= 0.5 ))
598
+ @test 10 < sum (isapprox .(Ref (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () )), pnts, Ref (ArrayPartition ([- 1.0 ,0.0 ], [1.0 0 ; 0 1 ])), atol= 0.5 ))
599
+ @test 10 < sum (isapprox .(Ref (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () )), pnts, Ref (ArrayPartition ([1.0 ,0.0 ], [1.0 0 ; 0 1 ])), atol= 0.5 ))
600
600
end
601
601
602
602
# #
603
603
end
604
604
605
- @testset " Test SpecialEuclidean(2) to SpecialEuclidean(2) multihypo" begin
605
+ @testset " Test SpecialEuclidean(2; vectors=HybridTangentRepresentation()) to SpecialEuclidean(2; vectors=HybridTangentRepresentation() ) multihypo" begin
606
606
# #
607
607
608
608
fg = initfg ()
609
609
# fg.solverParams.attemptGradients=false
610
610
611
611
v0 = addVariable! (fg, :x0 , SpecialEuclidean2)
612
612
613
- # mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
614
- mp = ManifoldPrior (SpecialEuclidean (2 ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
613
+ # mp = ManifoldPrior(SpecialEuclidean(2; vectors=HybridTangentRepresentation() ), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
614
+ mp = ManifoldPrior (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), ArrayPartition (Vector ([0.0 ,0.0 ]), Matrix ([1.0 0.0 ; 0.0 1.0 ])), MvNormal ([0.01 , 0.01 , 0.01 ]))
615
615
p = addFactor! (fg, [:x0 ], mp)
616
616
617
617
# #
618
618
addVariable! (fg, :x1a , SpecialEuclidean2)
619
619
addVariable! (fg, :x1b , SpecialEuclidean2)
620
- mf = ManifoldFactor (SpecialEuclidean (2 ), MvNormal ([1 ,2 ,pi / 4 ], [0.01 ,0.01 ,0.01 ]))
620
+ mf = ManifoldFactor (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), MvNormal ([1 ,2 ,pi / 4 ], [0.01 ,0.01 ,0.01 ]))
621
621
f = addFactor! (fg, [:x0 , :x1a , :x1b ], mf; multihypo= [1 ,0.5 ,0.5 ])
622
622
623
623
solveTree! (fg)
624
624
625
625
vnd = getVariableSolverData (fg, :x0 )
626
- @test isapprox (SpecialEuclidean (2 ) , mean (SpecialEuclidean (2 ), vnd. val), ArrayPartition ([0.0 ,0.0 ], [1.0 0 ; 0 1 ]), atol= 0.1 )
626
+ @test isapprox (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation ()) , mean (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () ), vnd. val), ArrayPartition ([0.0 ,0.0 ], [1.0 0 ; 0 1 ]), atol= 0.1 )
627
627
628
628
# FIXME I would expect close to 50% of particles to land on the correct place
629
629
# Currently software works so that 33% should land there so testing 20 for now
630
630
pnt = getPoints (fg, :x1a )
631
- @test sum (isapprox .(Ref (SpecialEuclidean (2 )), pnt, Ref (ArrayPartition ([1.0 ,2.0 ], [0.7071 - 0.7071 ; 0.7071 0.7071 ])), atol= 0.1 )) > 20
631
+ @test sum (isapprox .(Ref (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () )), pnt, Ref (ArrayPartition ([1.0 ,2.0 ], [0.7071 - 0.7071 ; 0.7071 0.7071 ])), atol= 0.1 )) > 20
632
632
633
633
# FIXME I would expect close to 50% of particles to land on the correct place
634
634
pnt = getPoints (fg, :x1b )
635
- @test sum (isapprox .(Ref (SpecialEuclidean (2 )), pnt, Ref (ArrayPartition ([1.0 ,2.0 ], [0.7071 - 0.7071 ; 0.7071 0.7071 ])), atol= 0.1 )) > 20
635
+ @test sum (isapprox .(Ref (SpecialEuclidean (2 ; vectors = HybridTangentRepresentation () )), pnt, Ref (ArrayPartition ([1.0 ,2.0 ], [0.7071 - 0.7071 ; 0.7071 0.7071 ])), atol= 0.1 )) > 20
636
636
637
637
# #
638
638
end
0 commit comments