wip fixing tests for staticarrays

Author: dehann

Project.toml

@@ -56,7 +56,7 @@ ApproxManifoldProducts = "0.7"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
-DistributedFactorGraphs = "0.21"
+DistributedFactorGraphs = "0.21, 0.22"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
 FileIO = "1"
@@ -91,10 +91,11 @@ Flux = "587475ba-b771-5e3f-ad9e-33799f191a9c"
 Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6"
 InteractiveUtils = "b77e0a4c-d291-57a0-90e8-8db25a27a240"
 Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59"
+LineSearches = ""d3d80556-e9d4-5f37-9878-2ab0fcc64255"
 Manopt = "0fc0a36d-df90-57f3-8f93-d78a9fc72bb5"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["DifferentialEquations", "Flux", "Graphs", "Manopt", "InteractiveUtils", "Interpolations", "Pkg", "Rotations", "Test"]
+test = ["DifferentialEquations", "Flux", "Graphs", "Manopt", "InteractiveUtils", "Interpolations", "LineSearches", "Pkg", "Rotations", "Test"]

src/ManifoldSampling.jl

@@ -30,7 +30,7 @@ end
 function sampleTangent(
   M::AbstractDecoratorManifold,
   z::Distribution,
-  p = getPointIdentity(M),
+  p = identity_element(M), #getPointIdentity(M),
 )
   return hat(M, p, rand(z, 1)[:]) #TODO find something better than (z,1)[:]
 end

src/VariableStatistics.jl

@@ -18,8 +18,13 @@ function Statistics.cov(
   return cov(getManifold(vartype), ptsArr; basis, kwargs...)
 end
 
-function calcStdBasicSpread(vartype::InferenceVariable, ptsArr::Vector{P}) where {P}
-  σ = std(vartype, ptsArr)
+function calcStdBasicSpread(vartype::InferenceVariable, ptsArr::AbstractVector) # {P}) where {P}
+  _makemutable(s) = s
+  _makemutable(s::StaticArray{Tuple{S},T,N}) where {S,T,N} = MArray{Tuple{S},T,N,S}(s)
+  _makemutable(s::SMatrix{N,N,T,D}) where {N,T,D} = MMatrix{N,N,T,D}(s)
+  
+  # silly conversion since Manifolds.std internally replicates eltype ptsArr which doesn't work on StaticArrays
+  σ = std(vartype, _makemutable.(ptsArr))
 
   #if no std yet, set to 1
   msst = 1e-10 < σ ? σ : 1.0

test/testBasicManifolds.jl

@@ -15,7 +15,7 @@ M = SpecialEuclidean(3)
 Mr = M.manifold[2]
 pPq = ArrayPartition(zeros(3), exp(Mr, Identity(Mr), hat(Mr, Identity(Mr), w)))
 rPc_ = exp(M, Identity(M), hat(M, Identity(M), [zeros(3);w]))
-rPc = ArrayPartition(rPc_.parts[1], rPc_.parts[2])
+rPc = ArrayPartition(rPc_.x[1], rPc_.x[2])
 
 @test isapprox(pPq.x[1], rPc.x[1])
 @test isapprox(pPq.x[2], rPc.x[2])

test/testSpecialOrthogonalMani.jl

@@ -1,5 +1,6 @@
 using DistributedFactorGraphs
 using IncrementalInference
+using LineSearches
 using Manifolds
 using StaticArrays
 using Test
@@ -14,13 +15,13 @@ Base.convert(::Type{<:Tuple}, M::SpecialOrthogonal{2}) = (:Circular,)
 Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{SpecialOrthogonal{2}})  = (:Circular,)
 
 # @defVariable SpecialOrthogonal2 SpecialOrthogonal(2) @MMatrix([1.0 0.0; 0.0 1.0])
-@defVariable SpecialOrthogonal2 SpecialOrthogonal(2) [1.0 0.0; 0.0 1.0]
+@defVariable SpecialOrthogonal2 SpecialOrthogonal(2) SMatrix{2,2}(1.0, 0.0, 0.0, 1.0)
 
 M = getManifold(SpecialOrthogonal2)
 @test M == SpecialOrthogonal(2)
 pT = getPointType(SpecialOrthogonal2)
 # @test pT == MMatrix{2, 2, Float64, 4}
-@test pT == Matrix{Float64}
+@test pT == SMatrix{2,2,Float64,4}
 pϵ = getPointIdentity(SpecialOrthogonal2)
 @test pϵ == [1.0 0.0; 0.0 1.0]
 
@@ -43,6 +44,7 @@ vnd = getVariableSolverData(fg, :x0)
 
 
 ##
+
 v1 = addVariable!(fg, :x1, SpecialOrthogonal2)
 mf = ManifoldFactor(SpecialOrthogonal(2), MvNormal([pi], [0.01]))
 f = addFactor!(fg, [:x0, :x1], mf)
@@ -69,13 +71,13 @@ Base.convert(::Type{<:Tuple}, M::SpecialOrthogonal{3}) = (:Euclid, :Euclid, :Euc
 Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{SpecialOrthogonal{3}})  =  (:Euclid, :Euclid, :Euclid)
 
 # @defVariable SO3 SpecialOrthogonal(3) @MMatrix([1.0 0.0; 0.0 1.0])
-@defVariable SO3 SpecialOrthogonal(3) [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]
+@defVariable SO3 SpecialOrthogonal(3) SMatrix{3,3}(1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0)
 
 M = getManifold(SO3)
 @test M == SpecialOrthogonal(3)
 pT = getPointType(SO3)
 # @test pT == MMatrix{2, 2, Float64, 4}
-@test pT == Matrix{Float64}
+@test pT == SMatrix{3,3,Float64,9}
 pϵ = getPointIdentity(SO3)
 @test pϵ == [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]
 
@@ -98,6 +100,7 @@ points = sampleFactor(fg, :x0f1, 100)
 std(SpecialOrthogonal(3), points)
 
 ##
+
 v1 = addVariable!(fg, :x1, SO3)
 mf = ManifoldFactor(SpecialOrthogonal(3), MvNormal([0.01,0.01,0.01], [0.01,0.01,0.01]))
 f = addFactor!(fg, [:x0, :x1], mf)
@@ -121,6 +124,10 @@ vnd = getVariableSolverData(fg, :x1)
 @test all(is_point.(Ref(M), vnd.val))
 
 
-IIF.solveGraphParametric!(fg)
+IIF.solveGraphParametric!(
+  fg;
+  algorithmkwargs=(;alphaguess = LineSearches.InitialStatic(), linesearch = LineSearches.MoreThuente())
+)
+
 ##
 end

test/testSphereMani.jl

@@ -4,22 +4,25 @@ using Manifolds
 using StaticArrays
 using Test
 
+import Manifolds: identity_element
+
 ##
 
 @testset "Test Sphere(2) prior and relative" begin
 ##
 
 #FIXME REMOVE! this is type piracy and not a good idea, for testing only!!!
-Manifolds.identity_element(::Sphere{2, ℝ}, p::Vector{Float64}) = Float64[1,0,0]
+Manifolds.identity_element(::Sphere{2, ℝ}) = SVector(1.0, 0.0, 0.0)
+Manifolds.identity_element(::Sphere{2, ℝ}, p::AbstractVector) = SVector(1.0, 0.0, 0.0) # Float64[1,0,0]
 
 Base.convert(::Type{<:Tuple}, M::Sphere{2, ℝ}) = (:Euclid, :Euclid)
 Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{Sphere{2, ℝ}})  = (:Euclid, :Euclid)
 
-@defVariable Sphere2 Sphere(2) [1.0, 0.0, 0.0]
+@defVariable Sphere2 Sphere(2) SVector(1.0, 0.0, 0.0)
 M = getManifold(Sphere2)
 @test M == Sphere(2)
 pT = getPointType(Sphere2)
-@test pT == Vector{Float64}
+@test pT == SVector{3,Float64}
 pϵ = getPointIdentity(Sphere2)
 @test pϵ == [1.0, 0.0, 0.0]