WIP Change points to StaticArrays

Author: Affie

src/ConsolidateParametricRelatives.jl

@@ -52,11 +52,12 @@ function solveFactorParameteric(
     # hasp ? getPPE(vari, key).suggested : calcMean(getBelief(vari, key))
     pt = calcMean(getBelief(vari, key))
 
-    return getCoordinates(getVariableType(vari), pt)
+    return collect(getCoordinates(getVariableType(vari), pt))
   end
 
   # overwrite specific src values from user
   coordVals = _getParametric.(getVariable.(dfg, varLbls), solveKey)
+
   for (srcsym, currval) in srcsym_vals
     coordVals[findfirst(varLbls .== srcsym)] = currval
   end

src/Deprecated.jl

@@ -1,3 +1,11 @@
+## ================================================================================================
+## ================================================================================================
+
+# TODO maybe upstream to DFG
+DFG.MeanMaxPPE(solveKey::Symbol, suggested::SVector, max::SVector, mean::SVector) =
+  DFG.MeanMaxPPE(solveKey, collect(suggested), collect(max), collect(mean))
+
+
 ## ================================================================================================
 ## Manifolds.jl Consolidation
 ## TODO: Still to be completed and tested.

src/Factors/Circular.jl

@@ -27,6 +27,11 @@ function (cf::CalcFactor{<:CircularCircular})(X, p, q)
   return distanceTangent2Point(M, X, p, q)
 end
 
+function getSample(cf::CalcFactor{<:CircularCircular})
+  # FIXME workaround for issue with manifolds CircularGroup, 
+  return [rand(cf.factor.Z)]
+end
+
 function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{CircularCircular})
   return Manifolds.RealCircleGroup()
 end
@@ -59,6 +64,7 @@ function getSample(cf::CalcFactor{<:PriorCircular})
   # JuliaManifolds/Manifolds.jl#415
   # no method similar(::Float64, ::Type{Float64})
   return samplePoint(cf.manifold, cf.factor.Z, [0.0])
+  # return [Manifolds.sym_rem(rand(cf.factor.Z))]
 end
 
 function (cf::CalcFactor{<:PriorCircular})(m, p)

src/ManifoldsExtentions.jl

@@ -132,21 +132,17 @@ function getPointIdentity(G::SemidirectProductGroup, ::Type{T} = Float64) where
   return ArrayPartition(np, hp)
 end
 
-#FIXME fix back to SA
 function getPointIdentity(G::SpecialOrthogonal{N}, ::Type{T} = Float64) where {N, T <: Real}
-  # return SMatrix{N,N, T}(I)
-  return Matrix{T}(I, N, N)
+  return SMatrix{N, N, T}(I)
 end
 
 function getPointIdentity(
   G::TranslationGroup{Tuple{N}},
   ::Type{T} = Float64,
 ) where {N, T <: Real}
-  # return zeros(SVector{N,T})
-  return zeros(T, N)
+  return zeros(SVector{N,T})
 end
 
 function getPointIdentity(G::RealCircleGroup, ::Type{T} = Float64) where {T <: Real}
-  # return zero(T)
-  return [zero(T)]
+  return zero(T)
 end

src/ParametricUtils.jl

@@ -52,7 +52,7 @@ end
 
 function Base.setindex!(
   flatVar::FlatVariables{T},
-  val::Vector{T},
+  val::AbstractVector{T},
   vId::Symbol,
 ) where {T <: Real}
   if length(val) == length(flatVar.idx[vId])
@@ -858,15 +858,15 @@ function initParametricFrom!(
     for v in getVariables(fg)
       fromvnd = getSolverData(v, fromkey)
       dims = getDimension(v)
-      getSolverData(v, parkey).val[1] .= fromvnd.val[1]
-      getSolverData(v, parkey).bw[1:dims, 1:dims] .= LinearAlgebra.I(dims)
+      getSolverData(v, parkey).val[1] = fromvnd.val[1]
+      getSolverData(v, parkey).bw[1:dims, 1:dims] = LinearAlgebra.I(dims)
     end
   else
     for var in getVariables(fg)
       dims = getDimension(var)
       μ, Σ = calcMeanCovar(var, fromkey)
-      getSolverData(var, parkey).val[1] .= μ
-      getSolverData(var, parkey).bw[1:dims, 1:dims] .= Σ
+      getSolverData(var, parkey).val[1] = μ
+      getSolverData(var, parkey).bw[1:dims, 1:dims] = Σ
     end
   end
 end
@@ -986,7 +986,7 @@ function autoinitParametric!(
 
     vnd.initialized = true
     #fill in ppe as mean
-    Xc = getCoordinates(getVariableType(xi), val)
+    Xc = collect(getCoordinates(getVariableType(xi), val))
     ppe = MeanMaxPPE(:parametric, Xc, Xc, Xc)
     getPPEDict(xi)[:parametric] = ppe
 

src/Variables/DefaultVariables.jl

@@ -50,5 +50,7 @@ $(TYPEDEF)
 Circular is a `Manifolds.Circle{ℝ}` mechanization of one rotation, with `theta in [-pi,pi)`.
 """
 @defVariable Circular RealCircleGroup() [0.0;]
+#TODO This is an example of what we want working, possible issue upstream in Manifolds.jl
+# @defVariable Circular RealCircleGroup() Scalar(0.0)
 
 #

src/services/EvalFactor.jl

@@ -370,16 +370,6 @@ function evalPotentialSpecific(
   return ccwl.varValsAll[ccwl.varidx[]], ipc
 end
 
-
-#TODO workaround for supporting bitstypes, need rewrite, can do with `PowerManifoldNestedReplacing` or similar
-function AMP.setPointsMani!(dest::AbstractArray{T}, src::AbstractArray{U}, destIdx, srcIdx=destIdx) where {T<:AbstractArray,U<:AbstractArray}
-  if isbitstype(T)
-    dest[destIdx] = src[srcIdx]
-  else
-    setPointsMani!(dest[destIdx],src[srcIdx])
-  end
-end
-
 # TODO `measurement` might not be properly wired up yet
 # TODO consider 1051 here to inflate proposals as general behaviour
 function evalPotentialSpecific(
@@ -477,10 +467,12 @@ function evalPotentialSpecific(
 
         setPointPartial!(
           mani,
-          addEntr[m],
+          addEntr,
           Msrc,
-          ccwl.measurement[m], # FIXME, measurements are tangents=>relative or points=>priors
+          ccwl.measurement, # FIXME, measurements are tangents=>relative or points=>priors
           partialCoords,
+          m,
+          m,
           asPartial,
         )
       else

src/services/NumericalCalculations.jl

@@ -98,7 +98,11 @@ function _solveLambdaNumeric(
   r = if islen1
     Optim.optimize((x) -> (residual .= objResX(x); sum(residual .^ 2)), u0, Optim.BFGS())
   else
-    Optim.optimize((x) -> (residual .= objResX(x); sum(residual .^ 2)), u0)
+    Optim.optimize((x) -> (residual .= objResX(x); sum(residual .^ 2)), u0, Optim.Options(;iterations=1000))
+  end
+
+  if !Optim.converged(r)
+    @warn "Optim did not converge:" r maxlog=10
   end
 
   # 
@@ -113,6 +117,20 @@ function _solveLambdaNumeric(
   variableType::InferenceVariable,
   islen1::Bool = false,
 ) where {N_, F <: AbstractManifoldMinimize, S, T}
+
+  return _solveCCWNumeric_test_SA(fcttype, objResX, residual, u0, variableType, islen1)
+  # return _solveLambdaNumeric_test_optim_manifold(fcttype, objResX, residual, u0, variableType, islen1)
+
+end
+
+function _solveLambdaNumeric_original(
+  fcttype::Union{F, <:Mixture{N_, F, S, T}},
+  objResX::Function,
+  residual::AbstractVector{<:Real},
+  u0,#::AbstractVector{<:Real},
+  variableType::InferenceVariable,
+  islen1::Bool = false,
+) where {N_, F <: AbstractManifoldMinimize, S, T}
   #
   M = getManifold(variableType) #fcttype.M
   # the variable is a manifold point, we are working on the tangent plane in optim for now.
@@ -153,6 +171,93 @@ function _solveLambdaNumeric(
   return exp(M, ϵ, hat(M, ϵ, r.minimizer))
 end
 
+# 1.355700 seconds (11.78 M allocations: 557.677 MiB, 6.96% gc time)
+function _solveCCWNumeric_test_SA(
+  fcttype::Union{F, <:Mixture{N_, F, S, T}},
+  objResX::Function,
+  residual::AbstractVector{<:Real},
+  u0,#::AbstractVector{<:Real},
+  variableType::InferenceVariable,
+  islen1::Bool = false,
+) where {N_, F <: AbstractManifoldMinimize, S, T}
+  #
+  M = getManifold(variableType) #fcttype.M
+  # the variable is a manifold point, we are working on the tangent plane in optim for now.
+  # 
+  #TODO this is not general to all manifolds, should work for lie groups.
+  # ϵ = identity_element(M, u0)
+  ϵ = getPointIdentity(variableType)
+
+  X0c = zero(MVector{getDimension(M),Float64})
+  X0c .= vee(M, u0, log(M, ϵ, u0))
+
+  #TODO check performance
+  function cost(Xc)
+    X = hat(M, ϵ, Xc)
+    p = exp(M, ϵ, X)  
+    residual = objResX(p)
+    return sum(residual .^ 2)
+  end
+
+  alg = islen1 ? Optim.BFGS() : Optim.NelderMead()
+
+  r = Optim.optimize(cost, X0c, alg)
+  if !Optim.converged(r)
+    # TODO find good way for a solve to store diagnostics about number of failed converges etc.
+    @warn "Optim did not converge (maxlog=10):" r maxlog=10
+  end
+  return exp(M, ϵ, hat(M, ϵ, r.minimizer))
+end
+
+# sloooowwww and does not always converge, unusable slow with gradient
+# NelderMead 5.513693 seconds (38.60 M allocations: 1.613 GiB, 6.62% gc time)
+function _solveLambdaNumeric_test_optim_manifold(
+  fcttype::Union{F, <:Mixture{N_, F, S, T}},
+  objResX::Function,
+  residual::AbstractVector{<:Real},
+  u0,#::AbstractVector{<:Real},
+  variableType::InferenceVariable,
+  islen1::Bool = false,
+) where {N_, F <: AbstractManifoldMinimize, S, T}
+  #
+  M = getManifold(variableType) #fcttype.M
+  # the variable is a manifold point, we are working on the tangent plane in optim for now.
+  # 
+  #TODO this is not general to all manifolds, should work for lie groups.
+  ϵ = getPointIdentity(variableType)
+
+  function cost(p)
+    residual = objResX(p)
+    return sum(residual .^ 2)
+  end
+
+  alg = islen1 ? Optim.BFGS(;manifold=ManifoldWrapper(M)) : Optim.NelderMead(;manifold=ManifoldWrapper(M))
+  # alg = Optim.ConjugateGradient(; manifold=ManifoldWrapper(M))
+  # alg = Optim.BFGS(; manifold=ManifoldWrapper(M))
+
+  # r_backend = ManifoldDiff.TangentDiffBackend(
+  #   ManifoldDiff.FiniteDifferencesBackend()
+  # )
+  
+  # ## finitediff gradient
+  # function costgrad_FD!(X,p)
+  #   copyto!(X, ManifoldDiff.gradient(M, cost, p, r_backend))
+  #   X
+  # end
+
+  u0_m = allocate(M, u0)
+  u0_m .= u0 
+  # r = Optim.optimize(cost, costgrad_FD!, u0_m, alg)
+  r = Optim.optimize(cost, u0_m, alg)
+  
+  if !Optim.converged(r)
+    @warn "Optim did not converge:" r maxlog=10
+  end
+
+  return r.minimizer
+  # return exp(M, ϵ, hat(M, ϵ, r.minimizer))
+end
+
 #TODO Consolidate with _solveLambdaNumeric, see #1374
 function _solveLambdaNumericMeas(
   fcttype::Union{F, <:Mixture{N_, F, S, T}},
@@ -317,15 +422,25 @@ function _solveCCWNumeric!(
   # islen1 = length(cpt_.X[:, smpid]) == 1 || ccwl.partial
 
   # build the pre-objective function for this sample's hypothesis selection
-  unrollHypo!, target = _buildCalcFactorLambdaSample(ccwl, smpid; _slack = _slack)
+  unrollHypo!, target = _buildCalcFactorLambdaSample(
+      ccwl, 
+      smpid, 
+      view(ccwl.varValsAll[ccwl.varidx[]], smpid);
+      _slack = _slack
+  )
 
   # broadcast updates original view memory location
   ## using CalcFactor legacy path inside (::CalcFactor)
-  _hypoObj = (x) -> (target .= x; unrollHypo!())
+
+  # _hypoObj = (x) -> (target[] = x; unrollHypo!())
+  function _hypoObj(x)
+    target[] = x
+    return unrollHypo!()
+  end
 
   # TODO small off-manifold perturbation is a numerical workaround only, make on-manifold requires RoME.jl #244
   # use all element dimensions : ==> 1:ccwl.xDim
-  target .+= _perturbIfNecessary(getFactorType(ccwl), length(target), perturb)
+  # target .+= _perturbIfNecessary(getFactorType(ccwl), length(target), perturb)
 
   sfidx = ccwl.varidx[]
   # do the parameter search over defined decision variables using Minimization
@@ -345,7 +460,7 @@ function _solveCCWNumeric!(
   end
 
   # insert result back at the correct variable element location
-  ccwl.varValsAll[sfidx][smpid][ccwl.partialDims] .= retval
+  copyto!(ccwl.varValsAll[sfidx][smpid][ccwl.partialDims], retval)
 
   return nothing
 end

test/testSpecialEuclidean2Mani.jl

@@ -9,10 +9,10 @@ import Rotations as _Rot
 
 ## define new local variable types for testing
 
-@defVariable TranslationGroup2 TranslationGroup(2) [0.0, 0.0]
+@defVariable TranslationGroup2 TranslationGroup(2) @SVector[0.0, 0.0]
 
-# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
-@defVariable SpecialEuclidean2 SpecialEuclidean(2) ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0])
+@defVariable SpecialEuclidean2 SpecialEuclidean(2) ArrayPartition(@SVector([0.0,0.0]), @SMatrix([1.0 0.0; 0.0 1.0]))
+# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0])
 
 ##
 
@@ -22,11 +22,12 @@ import Rotations as _Rot
 M = getManifold(SpecialEuclidean2)
 @test M == SpecialEuclidean(2)
 pT = getPointType(SpecialEuclidean2)
-@test pT == ArrayPartition{Float64,Tuple{Vector{Float64}, Matrix{Float64}}}
+# @test pT == ArrayPartition{Float64,Tuple{Vector{Float64}, Matrix{Float64}}}
 # @test pT == ArrayPartition{Tuple{MVector{2, Float64}, MMatrix{2, 2, Float64, 4}}}
+@test pT == ArrayPartition{Float64, Tuple{SVector{2, Float64}, SMatrix{2, 2, Float64, 4}}}
 pϵ = getPointIdentity(SpecialEuclidean2)
 # @test_broken pϵ == ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
-@test all(isapprox.(pϵ,ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0])))
+@test all(isapprox.(pϵ,ArrayPartition(SA[0.0,0.0], SA[1.0 0.0; 0.0 1.0])))
 
 @test is_point(getManifold(SpecialEuclidean2), getPointIdentity(SpecialEuclidean2))
 
@@ -38,6 +39,7 @@ v0 = addVariable!(fg, :x0, SpecialEuclidean2)
 # 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]))
 # 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]))))
 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]))))
+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]))))
 p = addFactor!(fg, [:x0], mp)
 
 
@@ -47,18 +49,18 @@ doautoinit!(fg, :x0)
 
 ##
 vnd = getVariableSolverData(fg, :x0)
-@test all(isapprox.(mean(vnd.val), ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0]), atol=0.1))
+@test all(isapprox.(mean(vnd.val), ArrayPartition(SA[0.0,0.0], SA[1.0 0.0; 0.0 1.0]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
 ##
 v1 = addVariable!(fg, :x1, SpecialEuclidean2)
-mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([1,2,pi/4], [0.01,0.01,0.01]))
+mf = ManifoldFactor(SpecialEuclidean(2), MvNormal(SA[1,2,pi/4], SA[0.01,0.01,0.01]))
 f = addFactor!(fg, [:x0, :x1], mf)
 
 doautoinit!(fg, :x1)
 
 vnd = getVariableSolverData(fg, :x1)
-@test all(isapprox(M, mean(M,vnd.val), ArrayPartition([1.0,2.0], [0.7071 -0.7071; 0.7071 0.7071]), atol=0.1))
+@test all(isapprox(M, mean(M,vnd.val), ArrayPartition(SA[1.0,2.0], SA[0.7071 -0.7071; 0.7071 0.7071]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
 ##
@@ -67,15 +69,15 @@ solveTree!(fg; smtasks, verbose=true) #, recordcliqs=ls(fg))
 # hists = fetchCliqHistoryAll!(smtasks);
 
 vnd = getVariableSolverData(fg, :x0)
-@test all(isapprox.(mean(vnd.val), ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0]), atol=0.1))
+@test all(isapprox.(mean(vnd.val), ArrayPartition(SA[0.0,0.0], SA[1.0 0.0; 0.0 1.0]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
 vnd = getVariableSolverData(fg, :x1)
-@test all(isapprox.(mean(vnd.val), ArrayPartition([1.0,2.0], [0.7071 -0.7071; 0.7071 0.7071]), atol=0.1))
+@test all(isapprox.(mean(vnd.val), ArrayPartition(SA[1.0,2.0], SA[0.7071 -0.7071; 0.7071 0.7071]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
 v1 = addVariable!(fg, :x2, SpecialEuclidean2)
-mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([1,2,pi/4], [0.01,0.01,0.01]))
+mf = ManifoldFactor(SpecialEuclidean(2), MvNormal(SA[1,2,pi/4], SA[0.01,0.01,0.01]))
 f = addFactor!(fg, [:x1, :x2], mf)
 
 ##