Merge pull request #1292 from JuliaRobotics/21Q2/enh/on_manifold_optim2

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,29 +3,6 @@ using IncrementalInference
 using StaticArrays
 using Manifolds
 
-## ======================================================================================
-## 
-## ======================================================================================
-@defVariable Pose2 SpecialEuclidean(2) IIF.default_identity(SpecialEuclidean(2))
-getManifold(Pose2)
-getPointType(Pose2)
-getPointIdentity(Pose2)
-
-fg = initfg()
-
-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]))
-p = addFactor!(fg, [:x0], mp;  graphinit=true)
-
-mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([0.1, 0.2, 0.01]))
-addFactor!(fg, [Symbol("x$i"),Symbol("x$(i+1)")], CircularCircular(Normal(1.0, 0.1))), 0:3)
-
-f1 = addFactor!(fg, [:x0,:x1])
-
-
 ## ======================================================================================
 ## 
 ## ======================================================================================
@@ -44,7 +21,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)
@@ -57,69 +34,3 @@ f = addFactor!(fg, [:x0, :x1], mf)
 
 
 solveGraph!(fg)
-
-## ======================================================================================
-## 
-## ======================================================================================
-
-# Base.convert(::Type{<:Tuple}, M::SpecialOrthogonal{2}) = (:Circular,)
-
-@defVariable SO2 SpecialOrthogonal(2) [0.0]
-getManifold(SO2)
-getPointType(SO2)
-getPointIdentity(SO2)
-
-fg = initfg()
-
-v0 = addVariable!(fg, :x0, SO2)
-v1 = addVariable!(fg, :x1, SO2)
-
-
-mp = ManifoldPrior(SpecialOrthogonal(2), MvNormal([1.0]), SA[1.0 0.0; 0 1])
-p = addFactor!(fg, [:x0], mp;  graphinit=true)
-
-doautoinit!(fg, :x0)
-
-# @enter addFactor!(fg, [:x0], mp;  graphinit=true)
-
-
-mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([0.1, 0.2, 0.01]))
-addFactor!(fg, [Symbol("x$i"),Symbol("x$(i+1)")], CircularCircular(Normal(1.0, 0.1))), 0:3)
-
-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)
-
-@defVariable Sphere2 Sphere(2) [1.0, 0.0, 0.0]
-M = getManifold(Sphere2)
-pT = getPointType(Sphere2)
-pϵ = getPointIdentity(Sphere2)
-
-is_point(getManifold(Sphere2), getPointIdentity(Sphere2))
-
-fg = initfg()
-
-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])
-p = addFactor!(fg, [:x0], mp)
-
-doautoinit!(fg, :x0)
-@enter doautoinit!(fg, :x0)
-
-# @enter addFactor!(fg, [:x0], mp;  graphinit=true)
-
-
-mf = ManifoldFactor(Sphere(2), MvNormal([1.0, 2.0], [0.1,0.1]))
-f = addFactor!(fg, [:x0, :x1], mf)
-
-
-solveGraph!(fg)

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.vartypes = typeof.(getVariableType.(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 .* transpose.(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(getManifold(ccwl.vartypes[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/CompareUtils.jl

@@ -28,7 +28,8 @@ function compareAllSpecial( A::T1, B::T2;
     @warn "CCW types not equal" T1 T2
     return false
   end
-  return compareAll(A, B, skip=skip, show=show)
+  # FIXME still issues with compare, skipping :vartypes https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/434
+  return compareAll(A, B, skip=union(skip, [:vartypes]), show=show)
 end
 
 

src/FGOSUtils.jl

@@ -261,11 +261,18 @@ function calcPPE( var::DFGVariable,
   manis = convert(Tuple, maniDef) # LEGACY, TODO REMOVE
   ops = buildHybridManifoldCallbacks(manis)
   Pme = calcMean(P)  # getKDEMean(P) #, addop=ops[1], diffop=ops[2]
-  Pma = getKDEMax(P, addop=ops[1], diffop=ops[2])
 
+  # returns coordinates at identify
+  Pma = getKDEMax(P, addop=ops[1], diffop=ops[2])
+  # calculate point
+
+  ## TODO make PPE only use getCoordinates for now (IIF v0.25)
+  Pme_ = getCoordinates(typeof(varType),Pme)
+  # Pma_ = getCoordinates(M,Pme)
+
   # suggested, max, mean, current time
   # TODO, poor constructor argument assumptions on `ppeType`
-  ppeType(ppeKey, Pme, Pma, Pme, timestamp)
+  ppeType(ppeKey, Pme_, Pma, Pme_, timestamp)
 end
 
 

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
@@ -654,7 +655,8 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
           nullhypo=nullhypo,
           threadmodel=threadmodel,
           inflation=inflation,
-          partialDims=partialDims
+          partialDims=partialDims,
+          vartypes = typeof.(getVariableType.(Xi))
         )
   #
   return ccw

src/FactorGraphTypes.jl

@@ -223,7 +223,7 @@ $(TYPEDEF)
 mutable struct CommonConvWrapper{ T<:FunctorInferenceType,
                                   H<:Union{Nothing, Distributions.Categorical},
                                   C<:Union{Nothing, Vector{Int}},
-                                  P } <: FactorOperationalMemory
+                                  P} <: FactorOperationalMemory
   #
   ### Values consistent across all threads during approx convolution
   usrfnc!::T # user factor / function
@@ -239,6 +239,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 +251,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}
+
+  # variable types for points in params
+  vartypes::Vector{DataType}
 end
 
 
@@ -272,7 +276,9 @@ function CommonConvWrapper( fnc::T,
                             perturb=zeros(zDim),
                             res::AbstractVector{<:Real}=zeros(zDim),
                             threadmodel::Type{<:_AbstractThreadModel}=MultiThreaded,
-                            inflation::Real=3.0  ) where {T<:FunctorInferenceType,P,H}
+                            inflation::Real=3.0,
+                            vartypes=typeof.(getVariableType.(factormetadata.fullvariables))
+                            ) where {T<:FunctorInferenceType,P,H}
   #
   return  CommonConvWrapper(fnc,
                             xDim,
@@ -290,8 +296,8 @@ function CommonConvWrapper( fnc::T,
                                               activehypo=activehypo, p=partialDims, 
                                               perturb=perturb, res=res )).(1:Threads.nthreads()),
                             inflation,
-                            partialDims  # SVector(Int32.()...)
-                            )
+                            partialDims,  # SVector(Int32.()...)
+                            vartypes)
 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]