Skip to content

Commit b4a6b50

Browse files
AffieAffie
authored
Merge pull request #1841 from JuliaRobotics/maint/mani_v0.10
Updates for manifolds v0.10
2 parents 03fb278 + 500c82c commit b4a6b50

File tree

9 files changed

+51
-51
lines changed

9 files changed

+51
-51
lines changed

‎Project.toml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -85,15 +85,15 @@ JSON3 = "1"
8585
KernelDensityEstimate = "0.5.6"
8686
LinearAlgebra = "1.10"
8787
ManifoldDiff = "0.3"
88-
Manifolds = "0.9.12"
88+
Manifolds = "0.10"
8989
ManifoldsBase = "0.15"
90-
Manopt = "0.4.40"
90+
Manopt = "0.4.40, 0.5"
9191
MetaGraphs = "0.7"
9292
Optim = "1"
9393
OrderedCollections = "1"
9494
PrecompileTools = "1"
9595
ProgressMeter = "1"
96-
RecursiveArrayTools = "2.31.1, 3"
96+
RecursiveArrayTools = "3"
9797
Reexport = "1"
9898
SparseDiffTools = "2"
9999
StaticArrays = "1"

‎src/services/FGOSUtils.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ function manikde!(
119119
variableType::Union{InstanceType{<:InferenceVariable}, InstanceType{<:AbstractFactor}},
120120
pts::AbstractVector{P};
121121
kw...,
122-
) where {P <: Union{<:AbstractArray, <:Number, <:Manifolds.ArrayPartition}}
122+
) where {P <: Union{<:AbstractArray, <:Number, <: ArrayPartition}}
123123
#
124124
M = getManifold(variableType)
125125
# @info "pts" P typeof(pts[1]) pts[1]

‎src/services/GraphInit.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -391,7 +391,7 @@ function initVariable!(
391391
# TODO ArrayPartition inputs
392392
_prodrepr(pt) = pt
393393
# _prodrepr(pt::Tuple) = Manifolds.ProductRepr(pt...)
394-
_prodrepr(pt::Tuple) = Manifolds.ArrayPartition(pt...)
394+
_prodrepr(pt::Tuple) = ArrayPartition(pt...)
395395

396396
M = getManifold(vari)
397397
pp = manikde!(M, _prodrepr.(pts); bw)

‎src/services/SolverAPI.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -342,7 +342,7 @@ function solveTree!(
342342
runtaskmonitor::Bool = true,
343343
algorithm::Symbol = :default,
344344
solveKey::Symbol = algorithm,
345-
multithread::Bool = true,
345+
multithread::Bool = false,
346346
)
347347
#
348348
# workaround in case isolated variables occur

‎test/basicGraphsOperations.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ varT = LinearRelative{1}
3030
manikde!(varT, pts)
3131

3232

33-
DFG.@defVariable _TestManiKde IIF.Manifolds.SpecialEuclidean(2) ArrayPartition([0;0.], [1 0; 0 1.])
33+
DFG.@defVariable _TestManiKde IIF.Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) ArrayPartition([0;0.], [1 0; 0 1.])
3434

3535
# construct directly with ArrayPartition
3636
pts = [ArrayPartition(randn(2), [1 0; 0 1.]) for _ in 1:100]

‎test/manifolds/factordiff.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ end
8383
##
8484

8585

86-
M = SpecialEuclidean(2)
86+
M = SpecialEuclidean(2; vectors=HybridTangentRepresentation())
8787
z = ArrayPartition(SA[10.0; 0.0], SMatrix{2,2}(0.0, -1.0, 1.0, 0.0))
8888

8989
p1 = ArrayPartition(SA[0.0; 0.0], SMatrix{2,2}(1, 0, 0, 1.))

‎test/manifolds/manifolddiff.jl‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -119,10 +119,10 @@ sol = Optim.optimize(f, g_FD!, x0, Optim.ConjugateGradient(; manifold=ManifoldWr
119119
end
120120

121121

122-
@testset "Modified Manifolds.jl ManifoldWrapper <: Optim.Manifold for SpecialEuclidean(2)" begin
122+
@testset "Modified Manifolds.jl ManifoldWrapper <: Optim.Manifold for SpecialEuclidean(2; vectors=HybridTangentRepresentation())" begin
123123
##
124124

125-
M = Manifolds.SpecialEuclidean(2)
125+
M = Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation())
126126
e0 = ArrayPartition([0,0.], [1 0; 0 1.])
127127

128128
x0 = deepcopy(e0)
@@ -171,7 +171,7 @@ end
171171
##
172172

173173

174-
M = Manifolds.SpecialEuclidean(3)
174+
M = Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation())
175175
e0 = ArrayPartition([0,0,0.], Matrix(_Rot.RotXYZ(0,0,0.)))
176176

177177
x0 = deepcopy(e0)
@@ -217,7 +217,7 @@ end
217217
@testset "Optim.Manifolds, SpecialEuclidean(3), using IIF.optimizeManifold_FD" begin
218218
##
219219

220-
M = Manifolds.SpecialEuclidean(3)
220+
M = Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation())
221221
e0 = ArrayPartition([0,0,0.], Matrix(_Rot.RotXYZ(0,0,0.)))
222222

223223
x0 = deepcopy(e0)

‎test/testBasicManifolds.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ using Test
1111

1212
w = [-0.0;-0.78;-0.18]
1313

14-
M = SpecialEuclidean(3)
14+
M = SpecialEuclidean(3; vectors=HybridTangentRepresentation())
1515
Mr = M.manifold[2]
1616
pPq = ArrayPartition(zeros(3), exp(Mr, Identity(Mr), hat(Mr, Identity(Mr), w)))
1717
rPc_ = exp(M, Identity(M), hat(M, Identity(M), [zeros(3);w]))

‎test/testSpecialEuclidean2Mani.jl‎

Lines changed: 38 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ import Rotations as _Rot
1111

1212
@defVariable TranslationGroup2 TranslationGroup(2) @SVector[0.0, 0.0]
1313

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]))
1515
# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0])
1616

1717
##
@@ -20,7 +20,7 @@ import Rotations as _Rot
2020
##
2121

2222
M = getManifold(SpecialEuclidean2)
23-
@test M == SpecialEuclidean(2)
23+
@test M == SpecialEuclidean(2; vectors=HybridTangentRepresentation())
2424
pT = getPointType(SpecialEuclidean2)
2525
# @test pT == ArrayPartition{Float64,Tuple{Vector{Float64}, Matrix{Float64}}}
2626
# @test pT == ArrayPartition{Tuple{MVector{2, Float64}, MMatrix{2, 2, Float64, 4}}}
@@ -38,8 +38,8 @@ v0 = addVariable!(fg, :x0, SpecialEuclidean2)
3838

3939
# 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]))
4040
# 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]))))
4343
p = addFactor!(fg, [:x0], mp)
4444

4545

@@ -54,7 +54,7 @@ vnd = getVariableSolverData(fg, :x0)
5454

5555
##
5656
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]))
5858
f = addFactor!(fg, [:x0, :x1], mf)
5959

6060
doautoinit!(fg, :x1)
@@ -77,7 +77,7 @@ vnd = getVariableSolverData(fg, :x1)
7777
@test all(is_point.(Ref(M), vnd.val))
7878

7979
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]))
8181
f = addFactor!(fg, [:x1, :x2], mf)
8282

8383
##
@@ -140,7 +140,7 @@ struct ManifoldFactorSE2{T <: SamplableBelief} <: IIF.AbstractManifoldMinimize
140140
end
141141

142142
ManifoldFactorSE2() = ManifoldFactorSE2(MvNormal(Diagonal([1,1,1])))
143-
DFG.getManifold(::ManifoldFactorSE2) = SpecialEuclidean(2)
143+
DFG.getManifold(::ManifoldFactorSE2) = SpecialEuclidean(2; vectors=HybridTangentRepresentation())
144144

145145
IIF.selectFactorType(::Type{<:SpecialEuclidean2}, ::Type{<:SpecialEuclidean2}) = ManifoldFactorSE2
146146

@@ -168,7 +168,7 @@ M = getManifold(SpecialEuclidean2)
168168
fg = initfg()
169169
v0 = addVariable!(fg, :x0, SpecialEuclidean2)
170170

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]))
172172
p = addFactor!(fg, [:x0], mp)
173173

174174
##
@@ -177,16 +177,16 @@ for i in 0:5
177177
psym = Symbol("x$i")
178178
nsym = Symbol("x$(i+1)")
179179
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]))
181181
f = addFactor!(fg, [psym;nsym], mf)
182182
end
183183

184184

185185
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]))
187187
addFactor!(fg, [:x0; :l1], mf)
188188

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]))
190190
addFactor!(fg, [:x6; :l1], mf)
191191

192192
##
@@ -225,7 +225,7 @@ getSolverParams(fg).useMsgLikelihoods = true
225225
addVariable!(fg, :x0, SpecialEuclidean2)
226226
addVariable!(fg, :x1, SpecialEuclidean2)
227227

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)))
229229
p = addFactor!(fg, [:x0], mp)
230230

231231
doautoinit!(fg,:x0)
@@ -238,7 +238,7 @@ initAll!(fg)
238238

239239
pred, meas = approxDeconv(fg, :x0x1f1)
240240

241-
@test mmd(SpecialEuclidean(2), pred, meas) < 1e-1
241+
@test mmd(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), pred, meas) < 1e-1
242242

243243
p_t = map(x->x.x[1], pred)
244244
m_t = map(x->x.x[1], meas)
@@ -278,7 +278,7 @@ DFG.getManifold(::ManiPose2Point2) = TranslationGroup(2)
278278
# define the conditional probability constraint
279279
function (cfo::CalcFactor{<:ManiPose2Point2})(measX, p, q)
280280
#
281-
M = SpecialEuclidean(2)
281+
M = SpecialEuclidean(2; vectors=HybridTangentRepresentation())
282282
q_SE = ArrayPartition(q, identity_element(SpecialOrthogonal(2), p.x[2]))
283283

284284
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)
288288
end
289289

290290
##
291-
@testset "Test SpecialEuclidean(2)" begin
291+
@testset "Test SpecialEuclidean(2; vectors=HybridTangentRepresentation())" begin
292292
##
293293

294294
# Base.convert(::Type{<:Tuple}, M::TranslationGroup{Tuple{2},ℝ}) = (:Euclid, :Euclid)
@@ -299,7 +299,7 @@ fg = initfg()
299299

300300
v0 = addVariable!(fg, :x0, SpecialEuclidean2)
301301

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]))
303303
p = addFactor!(fg, [:x0], mp)
304304

305305
##
@@ -413,7 +413,7 @@ solveGraph!(fg; smtasks);
413413

414414
##
415415

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.]))
417417
f1 = addFactor!(fg, [:x0], mp, graphinit=false)
418418

419419
@test length(ls(fg, :x0)) == 2
@@ -471,15 +471,15 @@ f0 = addFactor!(fg, [:x0], pthru, graphinit=false)
471471

472472
## test the inference functions
473473
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]))
476476
f1 = addFactor!(fg, [:x1], mp, graphinit=false)
477477

478478
doautoinit!(fg, :x1)
479479

480480
## connect with relative and check calculation size on x0
481481

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]))
483483
f2 = addFactor!(fg, [:x0, :x1], mf, graphinit=false)
484484

485485
##
@@ -507,10 +507,10 @@ hmd = LevelSetGridNormal(img_, (x_,y_), 5.5, 0.1, N=120)
507507
pthru = PartialPriorPassThrough(hmd, (1,2))
508508
f0 = addFactor!(fg, [:x0], pthru, graphinit=false)
509509
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]))
512512
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]))
514514
f2 = addFactor!(fg, [:x0, :x1], mf, graphinit=false)
515515

516516
##
@@ -531,16 +531,16 @@ initAll!(fg)
531531
end
532532

533533

534-
@testset "Test SpecialEuclidean(2) to TranslationGroup(2) multihypo" begin
534+
@testset "Test SpecialEuclidean(2; vectors=HybridTangentRepresentation()) to TranslationGroup(2) multihypo" begin
535535
##
536536

537537
fg = initfg()
538538
# fg.solverParams.attemptGradients=false
539539

540540
v0 = addVariable!(fg, :x0, SpecialEuclidean2)
541541

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]))
544544
p = addFactor!(fg, [:x0], mp)
545545

546546
##
@@ -552,7 +552,7 @@ f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
552552
solveTree!(fg)
553553

554554
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)
556556

557557
#FIXME I would expect close to 50% of particles to land on the correct place
558558
# Currently software works so that 33% should land there so testing 20 for now
@@ -573,8 +573,8 @@ addVariable!(fg, :x0, SpecialEuclidean2)
573573
addVariable!(fg, :x1a, TranslationGroup2)
574574
addVariable!(fg, :x1b, TranslationGroup2)
575575

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])))
578578
p = addFactor!(fg, [:x0], mp)
579579
mp = ManifoldPrior(TranslationGroup(2), [1.,0], MvNormal([0.01, 0.01]))
580580
p = addFactor!(fg, [:x1a], mp)
@@ -595,44 +595,44 @@ pnts = getPoints(fg, :x0)
595595
@error "Invalid multihypo test"
596596
if false
597597
# 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))
600600
end
601601

602602
##
603603
end
604604

605-
@testset "Test SpecialEuclidean(2) to SpecialEuclidean(2) multihypo" begin
605+
@testset "Test SpecialEuclidean(2; vectors=HybridTangentRepresentation()) to SpecialEuclidean(2; vectors=HybridTangentRepresentation()) multihypo" begin
606606
##
607607

608608
fg = initfg()
609609
# fg.solverParams.attemptGradients=false
610610

611611
v0 = addVariable!(fg, :x0, SpecialEuclidean2)
612612

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]))
615615
p = addFactor!(fg, [:x0], mp)
616616

617617
##
618618
addVariable!(fg, :x1a, SpecialEuclidean2)
619619
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]))
621621
f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
622622

623623
solveTree!(fg)
624624

625625
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)
627627

628628
#FIXME I would expect close to 50% of particles to land on the correct place
629629
# Currently software works so that 33% should land there so testing 20 for now
630630
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
632632

633633
#FIXME I would expect close to 50% of particles to land on the correct place
634634
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
636636

637637
##
638638
end

0 commit comments

Comments
 (0)