WIP on manifold optimization and factors

examples/dev/factors_sandbox.jl

@@ -16,7 +16,7 @@ function grad_PointPoint_distance(M, m, p, q)
 end
 
 function PointPoint_distance(M, X, p, q, ::MeasurementOnTangent)
-    q̂ = compose(M, p, exp(M, identity(M, X), X)) #FIXME
+    q̂ = compose(M, p, exp(M, identity(M, p), X))
     return distance(M, q, q̂)
 end
 

examples/dev/testManiFactors.jl

@@ -3,6 +3,24 @@ using IncrementalInference
 using StaticArrays
 using Manifolds
 
+
+##
+
+
+
+fg = initfg()
+
+v0 = addVariable!(fg, :x0, Pose2)
+v1 = addVariable!(fg, :x1, Point2)
+v2 = addVariable!(fg, :x2, Pose2)
+
+fg.solverParams.graphinit = false
+
+addFactor!(fg, [:x0], Prior(Normal()))
+addFactor!(fg, [:x0,:x1], Prior(Normal()))
+addFactor!(fg, [:x1,:x2], Prior(Normal()))
+addFactor!(fg, [:x0,:x2], Prior(Normal()))
+
 ## ======================================================================================
 ## 
 ## ======================================================================================
@@ -17,7 +35,7 @@ v0 = addVariable!(fg, :x0, Pose2)
 v1 = addVariable!(fg, :x1, Pose2)
 
 
-mp = ManifoldPrior(SpecialEuclidean(2), MvNormal([1.0, 1.0, 0.0]), ProductRepr(SA[0., 0], SA[1.0 0; 0 1]))
+mp = ManifoldPrior(SpecialEuclidean(2),ProductRepr(SA[0., 0], SA[1.0 0; 0 1]), MvNormal([1.0, 1.0, 0.0]))
 p = addFactor!(fg, [:x0], mp;  graphinit=true)
 
 mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([0.1, 0.2, 0.01]))
@@ -44,7 +62,7 @@ v0 = addVariable!(fg, :x0, Point2)
 v1 = addVariable!(fg, :x1, Point2)
 
 
-mp = ManifoldPrior(TranslationGroup(2), MvNormal([10.0, 20.0], [1.0,1.0]), SA[0., 0])
+mp = ManifoldPrior(TranslationGroup(2), SA[10., 20], MvNormal([1.0,1.0]))
 p = addFactor!(fg, [:x0], mp;  graphinit=true)
 
 doautoinit!(fg, :x0)
@@ -62,7 +80,7 @@ solveGraph!(fg)
 ## 
 ## ======================================================================================
 
-# Base.convert(::Type{<:Tuple}, M::SpecialOrthogonal{2}) = (:Circular,)
+Base.convert(::Type{<:Tuple}, M::SpecialOrthogonal{2}) = (:Circular,)
 
 @defVariable SO2 SpecialOrthogonal(2) [0.0]
 getManifold(SO2)
@@ -75,7 +93,7 @@ v0 = addVariable!(fg, :x0, SO2)
 v1 = addVariable!(fg, :x1, SO2)
 
 
-mp = ManifoldPrior(SpecialOrthogonal(2), MvNormal([1.0]), SA[1.0 0.0; 0 1])
+mp = ManifoldPrior(SpecialOrthogonal(2), SA[1.0 0.0; 0 1], MvNormal([1.0]))
 p = addFactor!(fg, [:x0], mp;  graphinit=true)
 
 doautoinit!(fg, :x0)
@@ -93,8 +111,8 @@ f1 = addFactor!(fg, [:x0,:x1])
 ## Sphere(n)
 ## ======================================================================================
 
-Base.convert(::Type{<:Tuple}, M::Sphere{2, ℝ}) = (:Euclid, :Euclid, :Euclid)
-Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{Sphere{2, ℝ}})  = (:Euclid, :Euclid, :Euclid)
+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]
 M = getManifold(Sphere2)
@@ -109,11 +127,11 @@ v0 = addVariable!(fg, :x0, Sphere2)
 v1 = addVariable!(fg, :x1, Sphere2)
 
 
-mp = ManifoldPrior(Sphere(2), MvNormal([0.0, 0.0], [0.01, 0.01]), SA[1., 0, 0])
+mp = ManifoldPrior(Sphere(2), SA[1., 0, 0], MvNormal([0.01, 0.01]))
 p = addFactor!(fg, [:x0], mp)
 
 doautoinit!(fg, :x0)
-@enter doautoinit!(fg, :x0)
+# @enter doautoinit!(fg, :x0)
 
 # @enter addFactor!(fg, [:x0], mp;  graphinit=true)
 

src/ApproxConv.jl

@@ -72,7 +72,13 @@ function prepareCommonConvWrapper!( F_::Type{<:AbstractRelative},
   #
 
   # FIXME, order of fmd ccwl cf are a little weird and should be revised.
-  vecPtsArr = Vector{Vector{Vector{Float64}}}()
+  pttypes = getVariableType.(Xi) .|> getPointType
+  PointType = 0 < length(pttypes) ? pttypes[1] : Vector{Float64}
+  vecPtsArr = Vector{Vector{PointType}}()
+
+  #TODO some better consolidate is needed
+  ccwl.manifolds = tuple(getManifold.(Xi)...)
+
   # FIXME maxlen should parrot N (barring multi-/nullhypo issues)
   maxlen, sfidx, mani = prepareparamsarray!(vecPtsArr, Xi, solvefor, N, solveKey=solveKey)
 
@@ -157,8 +163,8 @@ function generateNullhypoEntropy( val::AbstractMatrix{<:Real},
   MvNormal( mu, cVar )
 end
 
-# FIXME SWITCH TO ON-MANIFOLD VERSION
-function calcVariableCovarianceBasic(ptsArr::Vector{Vector{Float64}})
+# FIXME SWITCH TO ON-MANIFOLD VERSION (see next, #TODO remove this one if manifold version is correct)
+function calcVariableCovarianceBasic(M::AbstractManifold, ptsArr::Vector{Vector{Float64}})
   # cannot calculate the stdev from uninitialized state
   # FIXME assume point type is only Vector{Float} at this time
   @cast arr[i,j] := ptsArr[j][i]
@@ -168,6 +174,20 @@ function calcVariableCovarianceBasic(ptsArr::Vector{Vector{Float64}})
   return msst_
 end
 
+function calcVariableCovarianceBasic(M::AbstractManifold, ptsArr::Vector{P}) where P
+  #TODO double check the maths,. it looks like its working at least for groups
+  μ = mean(M, ptsArr)
+  Xcs = vee.(Ref(M), Ref(μ), log.(Ref(M), Ref(μ), ptsArr))
+  Σ = mean(Xcs .* adjoint.(Xcs))
+  @debug "calcVariableCovarianceBasic" μ
+  @debug "calcVariableCovarianceBasic" Σ
+  # TODO don't know what to do here so keeping as before, #FIXME it will break
+  # a change between this and previous is that a full covariance matrix is returned
+  msst = Σ
+  msst_ = 0 < sum(1e-10 .< msst) ? maximum(msst) : 1.0
+  return msst_
+end
+
 """
     $SIGNATURES
 
@@ -184,7 +204,7 @@ function calcVariableDistanceExpectedFractional(ccwl::CommonConvWrapper,
                                                 kappa::Float64=3.0  )
   #
   if sfidx in certainidx
-    msst_ = calcVariableCovarianceBasic(ccwl.params[sfidx])
+    msst_ = calcVariableCovarianceBasic(ccwl.manifolds[sfidx], ccwl.params[sfidx])
     return kappa*msst_
   end
   # @assert !(sfidx in certainidx) "null hypo distance does not work for sfidx in certainidx"
@@ -488,7 +508,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
   end
   # @show nn, size(addEntr), size(nhmask), size(solveForPts)
   addEntrNH = view(addEntr, nhmask)
-  spreadDist = spreadNH*calcVariableCovarianceBasic(addEntr)
+  spreadDist = spreadNH*calcVariableCovarianceBasic(mani, addEntr)
   # partials are treated differently
   if !ccwl.partial
       # TODO for now require measurements to be coordinates too

src/FactorGraph.jl

@@ -308,7 +308,8 @@ function DefaultNodeDataParametric( dodims::Int,
     #                         dims, false, :_null, Symbol[], variableType, true, 0.0, false, dontmargin)
   else
     sp = round.(Int,range(dodims,stop=dodims+dims-1,length=dims))
-    return VariableNodeData([zeros(dims) for _ in 1:1],
+    ϵ = getPointIdentity(variableType)
+    return VariableNodeData([ϵ],
                             zeros(dims,dims), Symbol[], sp,
                             dims, false, :_null, Symbol[], variableType, false, 0.0, false, dontmargin, 0, 0, :parametric)
   end

src/FactorGraphTypes.jl

@@ -223,7 +223,8 @@ $(TYPEDEF)
 mutable struct CommonConvWrapper{ T<:FunctorInferenceType,
                                   H<:Union{Nothing, Distributions.Categorical},
                                   C<:Union{Nothing, Vector{Int}},
-                                  P } <: FactorOperationalMemory
+                                  P,
+                                  M <: Tuple } <: FactorOperationalMemory
   #
   ### Values consistent across all threads during approx convolution
   usrfnc!::T # user factor / function
@@ -239,6 +240,7 @@ mutable struct CommonConvWrapper{ T<:FunctorInferenceType,
   certainhypo::C
   nullhypo::Float64
   # values specific to one complete convolution operation
+  # FIXME ? JT - What if all points are not on the same manifold?
   params::Vector{Vector{P}} # parameters passed to each hypothesis evaluation event on user function
   varidx::Int # which index is being solved for in params?
   # FIXME make type stable
@@ -250,6 +252,9 @@ mutable struct CommonConvWrapper{ T<:FunctorInferenceType,
   inflation::Float64
   # DONT USE THIS YET which dimensions does this factor influence
   partialDims::Vector{Int} # should become SVector{N, Int32}
+
+  # manifolds for points in params
+  manifolds::M
 end
 
 
@@ -290,8 +295,8 @@ function CommonConvWrapper( fnc::T,
                                               activehypo=activehypo, p=partialDims, 
                                               perturb=perturb, res=res )).(1:Threads.nthreads()),
                             inflation,
-                            partialDims  # SVector(Int32.()...)
-                            )
+                            partialDims,  # SVector(Int32.()...)
+                            tuple(getManifold.(factormetadata.fullvariables)...))
 end
 
 

src/Factors/GenericFunctions.jl

@@ -8,27 +8,31 @@ Generic function that can be used in binary factors to calculate distance betwee
 """
 function distancePoint2Point(M::AbstractGroupManifold, m, p, q)
     q̂ = compose(M, p, m)
-    return distance(M, q, q̂)
+    return log(M, q, q̂)
+    # return distance(M, q, q̂)
 end
 
 #::MeasurementOnTangent
 function distanceTangent2Point(M::AbstractGroupManifold, X, p, q)
     q̂ = compose(M, p, exp(M, identity(M, p), X)) #for groups
-    return distance(M, q, q̂)
+    return log(M, q, q̂)
+    # return distance(M, q, q̂)
 end
 
 # ::MeasurementOnTangent
 function distanceTangent2Point(M::AbstractManifold, X, p, q)
     q̂ = exp(M, p, X) 
-    return distance(M, q, q̂)
+    return log(M, q, q̂)
+    # return distance(M, q, q̂)
 end
 
 """
     $SIGNATURES
 Generic function that can be used in prior factors to calculate distance on Lie Groups. 
 """
 function distancePrior(M::AbstractManifold, meas, p)	
-    return distance(M, meas, p)
+    return log(M, p, meas)
+    # return distance(M, p, meas)
 end
 
 ## ======================================================================================
@@ -80,10 +84,11 @@ function getSample(cf::CalcFactor{<:ManifoldFactor}, N::Int=1)
     (ret, )
 end
 
-function (cf::CalcFactor{<:ManifoldFactor})(X, p, q)
+function (cf::CalcFactor{<:ManifoldFactor})(Xc, p, q)
 # function (cf::ManifoldFactor)(X, p, q)
     M = cf.factor.M
     # M = cf.M
+    X = hat(M, p, Xc)
     return distanceTangent2Point(M, X, p, q)
 end
 
@@ -95,8 +100,8 @@ export ManifoldPrior
 # `Z` is a measurement at the tangent space of `p` on manifold `M` 
 struct ManifoldPrior{M <: AbstractManifold, T <: SamplableBelief, P} <: AbstractPrior
     M::M 
+    p::P #NOTE This is a fixed point from where the measurement `Z` is made in coordinates on tangent TpM
     Z::T
-    p::P
 end
 
 #TODO
@@ -119,7 +124,8 @@ function getSample(cf::CalcFactor{<:ManifoldPrior}, N::Int=1)
 
     Xc = [rand(Z) for _ in 1:N]
 
-    X = get_vector.(Ref(M), Ref(p), Xc, Ref(DefaultOrthogonalBasis()))
+    # X = get_vector.(Ref(M), Ref(p), Xc, Ref(DefaultOrthogonalBasis()))
+    X = hat.(Ref(M), Ref(p), Xc)
     points = exp.(Ref(M), Ref(p), X)
 
     return (points, )
@@ -133,9 +139,22 @@ end
 function (cf::CalcFactor{<:ManifoldPrior})(m, p)
     M = cf.factor.M
     # M = cf.M
-    return distancePrior(M, m, p)
+    return log(M, p, m)
+    # return distancePrior(M, m, p)
 end
 
+# dist²_Σ = ⟨X, Σ⁻¹*X'⟩
+function mahalanobus_distance2(M, p, q, inv_Σ)
+    X = log(M, p, q)
+    return mahalanobus_distance2(Xc, inv_Σ)
+end
+
+function mahalanobus_distance2(M, X, inv_Σ)
+    #TODO look to replace with inner(MM, p, X, inv_Σ*X)
+    # Xc = get_coordinates(M, p, X, DefaultOrthogonalBasis())
+    Xc = vee(M, p, X)
+    return Xc' * inv_Σ * Xc
+end
 
 if false
 using IncrementalInference
@@ -152,16 +171,16 @@ s = getSample(f,10)[1]
 s[1]
 
 
-f = ManifoldPrior(SpecialOrthogonal(2), MvNormal([0.1]), SA[1.0 0; 0 1])
+f = ManifoldPrior(SpecialOrthogonal(2), SA[1.0 0; 0 1], MvNormal([0.1]))
 meas = getSample(f,10)[1]
 meas[1]
 f.(meas, Ref(SA[1.0 0; 0 1]))
 
-f = ManifoldPrior(SpecialOrthogonal(3), MvNormal([0.1, 0.02, 0.01]), SA[1.0 0 0; 0 1 0; 0 0 1])
+f = ManifoldPrior(SpecialOrthogonal(3), SA[1.0 0 0; 0 1 0; 0 0 1], MvNormal([0.1, 0.02, 0.01]))
 s = getSample(f,10)[1]
 s[1]
 
-f = ManifoldPrior(SpecialEuclidean(2), MvNormal([0.1, 0.2, 0.01]), ProductRepr(SA[0,0], SA[1.0 0; 0 1]))
+f = ManifoldPrior(SpecialEuclidean(2), ProductRepr(SA[0,0], SA[1.0 0; 0 1]), MvNormal([0.1, 0.2, 0.01]))
 s = getSample(f,10)[1]
 s[1]
 

src/NumericalCalculations.jl

@@ -81,17 +81,20 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
                               islen1::Bool=false  )  where {N_,F<:AbstractManifoldMinimize,S,T}
                               # retries::Int=3 )
   #
-
+  #TODO we need the point identity also
   M = fcttype.M
   # the variable is a manifold point, we are working on the tangent plane in optim for now.
   # 
   ϵ = identity(M, u0)
   # X0c = get_coordinates(M, u0, log(M, ϵ, u0), DefaultOrthogonalBasis()) 
   X0c = vee(M, u0, log(M, ϵ, u0)) 
 
+  # objResX(p) returns tangent vector at p, X=log(M, p, ...)
+  # norm(M, p, X) == distance(M, p, X)
   function cost(Xc)
-    x = exp(M, ϵ, hat(M, ϵ, Xc))  
-    return objResX(x)^2
+    p = exp(M, ϵ, hat(M, ϵ, Xc))  
+    X = objResX(p)
+    return norm(M, p, X)^2 #TODO verify 
   end
 
   alg = islen1 ? Optim.BFGS() : Optim.NelderMead() 
@@ -221,6 +224,12 @@ function _solveCCWNumeric!( ccwl::Union{CommonConvWrapper{F},
   #
   thrid = Threads.threadid()
   cpt_ = ccwl.cpt[thrid]
+
+  #FIXME hack to test manifold factors, cpt_.p should be full dimensions
+  if F <: AbstractManifoldMinimize
+    cpt_.p =  collect(1:length(cpt_.X[1]))
+  end
+
   smpid = cpt_.particleidx
   # cannot Nelder-Mead on 1dim, partial can be 1dim or more but being conservative.
   islen1 = length(cpt_.p) == 1 || ccwl.partial