Merge pull request #1290 from JuliaRobotics/21Q2/enh/on_manifold_optim

update addEntropyOnManifoldHack! add AbstractManifoldMinimize

examples/dev/testManiFactors.jl

@@ -0,0 +1,125 @@
+using DistributedFactorGraphs
+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])
+
+
+## ======================================================================================
+## 
+## ======================================================================================
+
+Base.convert(::Type{<:Tuple}, M::TranslationGroup{Tuple{2},ℝ}) = (:Euclid, :Euclid)
+Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{TranslationGroup{Tuple{2},ℝ}})  = (:Euclid, :Euclid)
+
+@defVariable Point2 TranslationGroup(2) [0.0, 0.0]
+getManifold(Point2)
+getPointType(Point2)
+getPointIdentity(Point2)
+
+fg = initfg()
+
+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])
+p = addFactor!(fg, [:x0], mp;  graphinit=true)
+
+doautoinit!(fg, :x0)
+
+# @enter addFactor!(fg, [:x0], mp;  graphinit=true)
+
+
+mf = ManifoldFactor(TranslationGroup(2), MvNormal([1.0, 2.0], [0.1,0.1]))
+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

@@ -231,22 +231,46 @@ function calcVariableDistanceExpectedFractional(ccwl::CommonConvWrapper,
   return kappa*maximum(dists)
 end
 
+# Add entrypy on a point in `points` on manifold M, only on dimIdx if in p 
 function addEntropyOnManifoldHack!( M::ManifoldsBase.AbstractManifold,
-                                    addEntr::Union{<:AbstractVector{<:Real},SubArray}, 
+                                    points::Union{<:AbstractVector{<:Real},SubArray}, 
                                     dimIdx::AbstractVector, 
                                     spreadDist::Real,
                                     p::Union{Colon, <:AbstractVector}=: )
   #
-  manis = convert(Tuple, M) # LEGACY, TODO REMOVE
-  # TODO deprecate
-  maniAddOps, _, _, _ = buildHybridManifoldCallbacks(manis)
-  # add 1σ "noise" level to max distance as control
-  # 1:size(addEntr, 1)
-  for dim in dimIdx, idx in 1:length(addEntr)
-    if (p === :) || dim in p
-      addEntr[idx][dim] = maniAddOps[dim](addEntr[idx][dim], spreadDist*(rand()-0.5))
+  if length(points) == 0 
+    return nothing
+  end
+
+  # preallocate 
+  T = number_eltype(points[1])
+  Xc = zeros(T, manifold_dimension(M))
+  X = get_vector(M, points[1], Xc, DefaultOrthogonalBasis())  
+
+  for idx in 1:length(points)
+    # build tangent coordinate random
+    for dim in dimIdx
+      if (p === :) || dim in p
+        Xc[dim] = spreadDist*(rand()-0.5)
+      end
     end
+    # update tangent vector X
+    get_vector!(M, X, points[idx], Xc, DefaultOrthogonalBasis())  
+    #update point
+    exp!(M, points[idx], points[idx], X)
+
   end
+  #
+  # manis = convert(Tuple, M) # LEGACY, TODO REMOVE
+  # # TODO deprecate
+  # maniAddOps, _, _, _ = buildHybridManifoldCallbacks(manis)
+  # # add 1σ "noise" level to max distance as control
+  # # 1:size(addEntr, 1)
+  # for dim in dimIdx, idx in 1:length(addEntr)
+  #   if (p === :) || dim in p
+  #     addEntr[idx][dim] = maniAddOps[dim](addEntr[idx][dim], spreadDist*(rand()-0.5))
+  #   end
+  # end
   nothing
 end
 
@@ -334,7 +358,7 @@ end
 Internal method to set which dimensions should be used as the decision variables for later numerical optimization.
 """
 function _setCCWDecisionDimsConv!(ccwl::Union{CommonConvWrapper{F},
-                                              CommonConvWrapper{Mixture{N_,F,S,T}}} ) where {N_,F<:Union{AbstractRelativeMinimize, AbstractRelativeRoots, AbstractPrior},S,T}
+                                              CommonConvWrapper{Mixture{N_,F,S,T}}} ) where {N_,F<:Union{AbstractManifoldMinimize, AbstractRelativeMinimize, AbstractRelativeRoots, AbstractPrior},S,T}
   #
   # return nothing
 

src/FactorGraph.jl

@@ -607,6 +607,8 @@ function calcZDim(cf::CalcFactor{T}) where {T <: FunctorInferenceType}
   return zdim
 end
 
+calcZDim(cf::CalcFactor{<:ManifoldPrior}) = manifold_dimension(cf.factor.M)
+
 
 function prepgenericconvolution(Xi::Vector{<:DFGVariable},
                                 usrfnc::T;

src/Factors/GenericFunctions.jl

@@ -0,0 +1,170 @@
+# TODO under development - experimenting with type to work with manifolds
+## ======================================================================================
+## Generic manifold cost functions
+## ======================================================================================
+"""
+    $SIGNATURES
+Generic function that can be used in binary factors to calculate distance between points on Lie Groups with measurements.
+"""
+function distancePoint2Point(M::AbstractGroupManifold, m, p, q)
+    q̂ = compose(M, p, m)
+    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̂)
+end
+
+# ::MeasurementOnTangent
+function distanceTangent2Point(M::AbstractManifold, X, p, q)
+    q̂ = exp(M, p, X) 
+    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)
+end
+
+## ======================================================================================
+## Default Identities #TODO only development, replace with better idea
+## ======================================================================================
+
+default_identity(M) = error("No default identity element defined for $(typeof(M))")
+default_identity(M::GroupManifold{ℝ, <:ProductManifold}) = error("No default identity element defined for $(typeof(M))")
+function default_identity(::SpecialEuclidean{N}) where N
+    T = Float64
+    t = zeros(SVector{N, T})
+    R = SMatrix{N,N,T}(one(T)I)
+    return ProductRepr(t, R)
+end
+function default_identity(M::GroupManifold{ℝ, <:AbstractManifold})
+    T = Float64
+    s = representation_size(M)
+    return identity(M, zeros(SArray{Tuple{s...},T}))
+end
+
+# function default_identity(::SpecialOrthogonal{N}) where N
+#     T = Float64
+#     return SMatrix{N,N,T}(one(T)I)
+# end
+
+## ======================================================================================
+## ManifoldFactor
+## ======================================================================================
+abstract type AbstractManifoldMinimize <: AbstractRelative end
+
+export ManifoldFactor
+# DEV NOTES
+# For now, `Z` is on the tangent space in coordinates at the point used in the factor.
+# For groups just the lie algebra
+# As transition it will be easier this way, we can reevaluate
+struct ManifoldFactor{M <: AbstractManifold, T <: SamplableBelief} <: AbstractManifoldMinimize#AbstractFactor
+    M::M
+    Z::T
+end
+
+# function getSample(cf::ManifoldFactor, N::Int=1)
+function getSample(cf::CalcFactor{<:ManifoldFactor}, N::Int=1)
+    #TODO @assert dim == cf.factor.Z's dimension
+    #TODO investigate use of SVector if small dims
+    ret = [rand(cf.factor.Z) for _ in 1:N]
+
+    #TODO tangent or not?
+    # tangent for now to fit with rest
+    (ret, )
+end
+
+function (cf::CalcFactor{<:ManifoldFactor})(X, p, q)
+# function (cf::ManifoldFactor)(X, p, q)
+    M = cf.factor.M
+    # M = cf.M
+    return distanceTangent2Point(M, X, p, q)
+end
+
+## ======================================================================================
+## ManifoldPrior
+## ======================================================================================
+export ManifoldPrior
+# `p` is a point on manifold `M`
+# `Z` is a measurement at the tangent space of `p` on manifold `M` 
+struct ManifoldPrior{M <: AbstractManifold, T <: SamplableBelief, P} <: AbstractPrior
+    M::M 
+    Z::T
+    p::P
+end
+
+#TODO
+# function ManifoldPrior(M::AbstractGroupManifold, Z::SamplableBelief)
+#     # p = identity(M, #TOOD)
+#     # similar to getPointIdentity(M)
+#     return ManifoldPrior(M, Z, p)
+# end
+
+# ManifoldPrior{M}(Z::SamplableBelief, p) where M = ManifoldPrior{M, typeof(Z), typeof(p)}(Z, p)
+
+# function getSample(cf::ManifoldPrior, N::Int=1)
+function getSample(cf::CalcFactor{<:ManifoldPrior}, N::Int=1)
+    Z = cf.factor.Z
+    p = cf.factor.p
+    M = cf.factor.M
+    # Z = cf.Z
+    # p = cf.p
+    # M = cf.M
+
+    Xc = [rand(Z) for _ in 1:N]
+
+    X = get_vector.(Ref(M), Ref(p), Xc, Ref(DefaultOrthogonalBasis()))
+    points = exp.(Ref(M), Ref(p), X)
+
+    return (points, )
+end
+
+#TODO investigate SVector if small dims, this is slower
+# dim = manifold_dimension(M)
+# Xc = [SVector{dim}(rand(Z)) for _ in 1:N]
+
+# function (cf::ManifoldPrior)(m, p)
+function (cf::CalcFactor{<:ManifoldPrior})(m, p)
+    M = cf.factor.M
+    # M = cf.M
+    return distancePrior(M, m, p)
+end
+
+
+if false
+using IncrementalInference
+using Manifolds
+using LinearAlgebra
+using StaticArrays
+
+f = ManifoldFactor(SpecialOrthogonal(3), MvNormal([0.1, 0.02, 0.01]))
+s = getSample(f,10)[1]
+s[1]
+
+f = ManifoldFactor(SpecialEuclidean(2), MvNormal([0.1, 0.2, 0.01]))
+s = getSample(f,10)[1]
+s[1]
+
+
+f = ManifoldPrior(SpecialOrthogonal(2), MvNormal([0.1]), SA[1.0 0; 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])
+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]))
+s = getSample(f,10)[1]
+s[1]
+
+
+
+end

src/IncrementalInference.jl

@@ -416,6 +416,9 @@ include("entities/OptionalDensities.jl")
 include("BeliefTypes.jl")
 include("CalcFactor.jl")
 
+
+include("Factors/GenericFunctions.jl")
+
 # Refactoring in progress
 include("Factors/MsgLikelihoods.jl")