Merge pull request #1725 from JuliaRobotics/23Q2/twig/manopt

dehann · web-flow · commit 5f2c0d848070 · 2023-07-03T15:56:03.000-07:00
wip w partials, sampleTangent
diff --git a/Project.toml b/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"]
diff --git a/src/Factors/GenericFunctions.jl b/src/Factors/GenericFunctions.jl
@@ -80,12 +80,14 @@ DFG.getManifold(f::ManifoldFactor) = f.M
 function getSample(cf::CalcFactor{<:ManifoldFactor{M, Z}}) where {M, Z}
   #TODO @assert dim == cf.factor.Z's dimension
   #TODO investigate use of SVector if small dims
-  if M isa ManifoldKernelDensity
-    ret = sample(cf.factor.Z.belief)[1]
-  else
-    ret = rand(cf.factor.Z)
-  end
-  # ret = sampleTangent(M, cf.factor.Z)
+  # if M isa ManifoldKernelDensity
+  #   ret = sample(cf.factor.Z.belief)[1]
+  # else
+  #   ret = rand(cf.factor.Z)
+  # end
+
+  # ASSUME this function is only used for RelativeFactors which must use measurements as tangents
+  ret = sampleTangent(cf.factor.M, cf.factor.Z)
   #return coordinates as we do not know the point here #TODO separate Lie group
   return ret
 end
diff --git a/src/ManifoldSampling.jl b/src/ManifoldSampling.jl
@@ -23,14 +23,15 @@ function sampleTangent(x::ManifoldKernelDensity, p = mean(x))
 end
 
 # Sampling Distributions
-function sampleTangent(M::AbstractManifold, z::Distribution, p, basis::AbstractBasis)
+# assumes M is a group and will break for Riemannian, but leaving that enhancement as TODO
+function sampleTangent(M::AbstractManifold, z::Distribution, p = getPointIdentity(M), basis::AbstractBasis = DefaultOrthogonalBasis())
   return get_vector(M, p, rand(z), basis)
 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
diff --git a/src/VariableStatistics.jl b/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)
+  
+  # FIXME, 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
diff --git a/src/Variables/DefaultVariables.jl b/src/Variables/DefaultVariables.jl
@@ -16,7 +16,7 @@ function DFG.getDimension(val::InstanceType{Position{N}}) where {N}
   return manifold_dimension(getManifold(val))
 end
 DFG.getPointType(::Type{Position{N}}) where {N} = Vector{Float64}
-DFG.getPointIdentity(M_::Type{Position{N}}) where {N} = zeros(N) # identity_element(getManifold(M_), zeros(N)) 
+DFG.getPointIdentity(M_::Type{Position{N}}) where {N} = @SVector(zeros(N)) # identity_element(getManifold(M_), zeros(N)) 
 
 function Base.convert(
   ::Type{<:ManifoldsBase.AbstractManifold},
diff --git a/src/services/FGOSUtils.jl b/src/services/FGOSUtils.jl
@@ -122,6 +122,7 @@ function manikde!(
 ) where {P <: Union{<:AbstractArray, <:Number, <:Manifolds.ArrayPartition}}
   #
   M = getManifold(variableType)
+  # @info "pts" P typeof(pts[1]) pts[1]
   infoPerCoord = ones(AMP.getNumberCoords(M, pts[1]))
   return AMP.manikde!(M, pts; infoPerCoord, kw...)
 end
diff --git a/test/manifolds/manifolddiff.jl b/test/manifolds/manifolddiff.jl
@@ -92,7 +92,7 @@ end
 ##
 
 sol = Optim.optimize(f, g!, x0, Optim.ConjugateGradient(; manifold=ManifoldWrapper(M)))
-@test isapprox([0,1,0.], sol.minimizer; atol=1e-8)
+@test isapprox([0,1,0.], sol.minimizer; atol=1e-6)
 
 
 ## finitediff gradient (non-manual)
@@ -106,7 +106,7 @@ end
 x0 = [1.0, 0.0, 0.0]
 
 sol = Optim.optimize(f, g_FD!, x0, Optim.ConjugateGradient(; manifold=ManifoldWrapper(M)))
-@test isapprox([0,1,0.], sol.minimizer; atol=1e-8)
+@test isapprox([0,1,0.], sol.minimizer; atol=1e-6)
 
 ##
 
@@ -161,7 +161,7 @@ Cq .= randn(3)
 # Cq[
 @show sol.minimizer
 @test isapprox( f(sol.minimizer), 0; atol=1e-3 )
-@test isapprox( 0, sum(abs.(log(M, e0, compose(M, inv(M,q), sol.minimizer)))); atol=1e-5)
+@test isapprox( 0, sum(abs.(log(M, e0, compose(M, inv(M,q), sol.minimizer)))); atol=1e-3)
 
 ##
 end
@@ -193,7 +193,7 @@ g_FD!(X, q)
 
 @show X_ = [X.x[1][:]; X.x[2][:]]
 # gradient at the optimal point should be zero
-@test isapprox(0, sum(abs.(X_)); atol=1e-8 )
+@test isapprox(0, sum(abs.(X_)); atol=1e-6 )
 
 # gradient not the optimal point should be non-zero
 g_FD!(X, e0)
@@ -230,7 +230,7 @@ sol = IncrementalInference.optimizeManifold_FD(M,f,x0)
 
 @show sol.minimizer
 @test isapprox( f(sol.minimizer), 0; atol=1e-3 )
-@test isapprox( 0, sum(abs.(log(M, e0, compose(M, inv(M,q), sol.minimizer)))); atol=1e-4)
+@test isapprox( 0, sum(abs.(log(M, e0, compose(M, inv(M,q), sol.minimizer)))); atol=1e-3)
 
 
 ##
diff --git a/test/testBasicManifolds.jl b/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])
diff --git a/test/testBasicParametric.jl b/test/testBasicParametric.jl
@@ -139,8 +139,8 @@ for i in 0:10
   sym = Symbol("x",i)
   var = getVariable(fg,sym)
   @show val = var.solverDataDict[:parametric].val
-  @test isapprox(val[1][1], i, atol=1e-4)
-  @test isapprox(val[1][2], i, atol=1e-4)
+  @test isapprox(val[1][1], i, atol=1e-3)
+  @test isapprox(val[1][2], i, atol=1e-3)
 end
 
 ##
@@ -179,9 +179,9 @@ foreach(fct->println(fct.label, ": ", getFactorType(fct).Z), getFactors(fg))
 d,st,vs,Σ = IIF.solveGraphParametric(fg)
 
 foreach(println, d)
-@test isapprox(d[:x0].val[1][1], -0.01, atol=1e-4)
-@test isapprox(d[:x1].val[1][1], 0.0, atol=1e-4)
-@test isapprox(d[:x2].val[1][1], 0.01, atol=1e-4)
+@test isapprox(d[:x0].val[1][1], -0.01, atol=1e-3)
+@test isapprox(d[:x1].val[1][1], 0.0, atol=1e-3)
+@test isapprox(d[:x2].val[1][1], 0.01, atol=1e-3)
 
 
 ##
@@ -202,9 +202,9 @@ tree2 = solveTree!(fg; algorithm=:parametric, eliminationOrder=[:x0, :x2, :x1])
 # end
 foreach(v->println(v.label, ": ", DFG.getSolverData(v, :parametric).val), getVariables(fg))
 
-@test isapprox(getVariable(fg,:x0).solverDataDict[:parametric].val[1][1], -0.01, atol=1e-4)
-@test isapprox(getVariable(fg,:x1).solverDataDict[:parametric].val[1][1], 0.0, atol=1e-4)
-@test isapprox(getVariable(fg,:x2).solverDataDict[:parametric].val[1][1], 0.01, atol=1e-4)
+@test isapprox(getVariable(fg,:x0).solverDataDict[:parametric].val[1][1], -0.01, atol=1e-3)
+@test isapprox(getVariable(fg,:x1).solverDataDict[:parametric].val[1][1], 0.0, atol=1e-3)
+@test isapprox(getVariable(fg,:x2).solverDataDict[:parametric].val[1][1], 0.01, atol=1e-3)
 
 ## ##############################################################################
 ## multiple sections
diff --git a/test/testSpecialEuclidean2Mani.jl b/test/testSpecialEuclidean2Mani.jl
@@ -388,9 +388,23 @@ doautoinit!(fg, :x0)
 @test length(getPoints(getBelief(fg, :x0))) == getSolverParams(fg).N # 120
 # @info "PassThrough transfers the full point count to the graph, unless a product is calculated during the propagateBelief step."
 
+
+
+## check the partials magic
+
+dens, ipc = propagateBelief(fg,:x0,:)
+testv = deepcopy(getVariable(fg, :x0))
+setBelief!(testv, dens, true, ipc)
+
+
 ##
 
-solveGraph!(fg);
+smtasks = Task[]
+solveGraph!(fg; smtasks);
+# hists = fetchCliqHistoryAll!(smtasks)
+# printCSMHistoryLogical(hists)
+# hists_ = deepcopy(hists)
+# repeatCSMStep!(hists, 1, 6)
 
 @test 120 == length(getPoints(fg, :x0))
 
diff --git a/test/testSpecialOrthogonalMani.jl b/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)
@@ -120,7 +123,11 @@ vnd = getVariableSolverData(fg, :x1)
 @test all(isapprox.( mean(SpecialOrthogonal(3),vnd.val), [0.9999 -0.00995 0.01005; 0.01005 0.9999 -0.00995; -0.00995 0.01005 0.9999], atol=0.01))
 @test all(is_point.(Ref(M), vnd.val))
 
+# 23Q2 default HagerZhang fails with `AssertionError: isfinite(phi_c) && isfinite(dphi_c)`, using alternate LineSearch
+IIF.solveGraphParametric!(
+  fg;
+  algorithmkwargs=(;alphaguess = LineSearches.InitialStatic(), linesearch = LineSearches.MoreThuente())
+)
 
-IIF.solveGraphParametric!(fg)
 ##
 end
diff --git a/test/testSphereMani.jl b/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]
 

Original file line number	Diff line number	Diff line change
`@@ -122,6 +122,7 @@ function manikde!(`
`122`	`122`	`) where {P <: Union{<:AbstractArray, <:Number, <:Manifolds.ArrayPartition}}`
`123`	`123`	`#`
`124`	`124`	`M = getManifold(variableType)`
	`125`	`+ # @info "pts" P typeof(pts[1]) pts[1]`
`125`	`126`	`infoPerCoord = ones(AMP.getNumberCoords(M, pts[1]))`
`126`	`127`	`return AMP.manikde!(M, pts; infoPerCoord, kw...)`
`127`	`128`	`end`