Merge pull request #1359 from JuliaRobotics/21Q3/enh/sampleTangent_Point

`sampleTangent` and `samplePoint`

Author: Affie

src/Factors/GenericFunctions.jl

@@ -38,29 +38,6 @@ function distancePrior(M::AbstractManifold, meas, p)
     # return distance(M, p, meas)
 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_element(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
 ## ======================================================================================
@@ -78,17 +55,15 @@ end
 
 DFG.getManifold(f::ManifoldFactor) = f.M
 
-# 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
+    #return coordinates as we do not know the point here #TODO separate Lie group
     (ret, )
 end
 
+# function (cf::CalcFactor{<:ManifoldFactor{<:AbstractGroupManifold}})(Xc, p, q)
 function (cf::CalcFactor{<:ManifoldFactor})(Xc, p, q)
 # function (cf::ManifoldFactor)(X, p, q)
     M = cf.factor.M
@@ -103,12 +78,16 @@ end
 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
+struct ManifoldPrior{M <: AbstractManifold, T <: SamplableBelief, P, B <: AbstractBasis} <: 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
+    basis::B
+    retract_method::AbstractRetractionMethod
 end
 
+ManifoldPrior(M::AbstractGroupManifold, p, Z) = ManifoldPrior(M, p, Z, ManifoldsBase.VeeOrthogonalBasis(), ExponentialRetraction())
+
 DFG.getManifold(f::ManifoldPrior) = f.M
 
 #TODO
@@ -120,32 +99,24 @@ DFG.getManifold(f::ManifoldPrior) = f.M
 
 # 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()))
-    X = hat.(Ref(M), Ref(p), Xc)
-    points = exp.(Ref(M), Ref(p), X)
+    basis = cf.factor.basis
+    retract_method = cf.factor.retract_method
 
+    points = [samplePoint(M, Z, p, basis, retract_method) for _=1:N]
+    
     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 log(M, p, m)
     return vee(M,p,log(M, p, m))
     # return distancePrior(M, m, p)
@@ -163,35 +134,3 @@ function mahalanobus_distance2(M, X, inv_Σ)
     Xc = vee(M, p, X)
     return Xc' * inv_Σ * Xc
 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), 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), 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), ProductRepr(SA[0,0], SA[1.0 0; 0 1]), MvNormal([0.1, 0.2, 0.01]))
-s = getSample(f,10)[1]
-s[1]
-
-
-
-end

src/IncrementalInference.jl

@@ -501,6 +501,8 @@ include("SolverAPI.jl")
 # Symbolic tree analysis files.
 include("AnalysisTools.jl")
 
+include("ManifoldSampling.jl")
+
 # deprecation legacy support
 include("Deprecated.jl")
 

src/ManifoldSampling.jl

@@ -0,0 +1,55 @@
+export sampleTangent
+export samplePoint
+"""
+    $SIGNATURES
+
+Return a random sample as a tangent vector from a belief represented by coordinates on a manifold at point p.
+
+Notes
+
+"""
+function sampleTangent end
+
+# Sampling MKD
+function sampleTangent(M::AbstractGroupManifold, x::ManifoldKernelDensity, p=mean(x))
+    # get legacy matrix of coordinates and selected labels
+    coords, lbls = sample(x.belief,1)
+    X = hat(x.manifold, p, coords)
+    return X
+end
+
+function sampleTangent(x::ManifoldKernelDensity, p=mean(x)) 
+  return sampleTangent(x.manifold, coords, p)
+end
+
+# Sampling Distributions
+function sampleTangent(M::AbstractManifold, z::Distribution, p, basis::AbstractBasis) 
+  return get_vector(M, p, rand(z), basis)
+end
+
+function sampleTangent(M::AbstractGroupManifold, z::Distribution, p=identity_element(M)) 
+  return hat(M, p, rand(z))
+end
+
+
+"""
+    $SIGNATURES
+
+Return a random sample point on a manifold from a belief represented by coordinates at point p.
+
+Notes
+
+"""
+function samplePoint(M::AbstractManifold, sbelief, p, basis, retraction_method::AbstractRetractionMethod=ExponentialRetraction())
+  X = sampleTangent(M, sbelief, p, basis)
+  return retract(M, p, X, retraction_method)
+end
+function samplePoint(M::AbstractGroupManifold, sbelief, p=identity_element(M), retraction_method::AbstractRetractionMethod=ExponentialRetraction())
+  X = sampleTangent(M, sbelief, p)
+  return retract(M, p, X, retraction_method)
+end
+
+
+## default getSample
+# getSample(cf::CalcFactor{<:AbstractPrior}, N::Int=1) = ([samplePoint(getManifold(cf.factor), cf.factor.Z, ) for _=1:N], )
+# getSample(cf::CalcFactor{<:AbstractRelative}, N::Int=1) = ([sampleTangent(getManifold(cf.factor), cf.factor.Z) for _=1:N], )

test/testSpecialEuclidean2Mani.jl

@@ -40,7 +40,7 @@ fg = initfg()
 v0 = addVariable!(fg, :x0, SpecialEuclidean2)
 
 # mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@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), ProductRepr(@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), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal(Diagonal(abs2.([0.01, 0.01, 0.01]))))
 p = addFactor!(fg, [:x0], mp)
 
 
@@ -109,6 +109,16 @@ pbel_ = approxConvBelief(fg, :x0f1, :x0)
 ##
 end
 
+struct ManifoldFactorSE2{T <: SamplableBelief} <: IIF.AbstractManifoldMinimize
+    Z::T
+end
+
+ManifoldFactorSE2() = ManifoldFactorSE2(MvNormal(Diagonal([1,1,1])))
+DFG.getManifold(::ManifoldFactorSE2) = SpecialEuclidean(2)
+
+IIF.selectFactorType(::Type{<:SpecialEuclidean2}, ::Type{<:SpecialEuclidean2}) = ManifoldFactorSE2
+
+
 @testset "Test Pose2 like hex as SpecialEuclidean2" begin
 ##
 
@@ -151,7 +161,13 @@ vnd = getVariableSolverData(fg, :x6)
 smtasks = Task[]
 solveTree!(fg; smtasks);
 
-##
+## Special test for manifold based messages
+
+#FIXME this may show some bug in propagateBelief caused by empty factors
+fg.solverParams.useMsgLikelihoods = true
+@test_broken solveTree!(fg; smtasks) isa Tuple
+
+
 end
 
 ## ======================================================================================

test/testSphereMani.jl

@@ -6,7 +6,7 @@ using Test
 
 ##
 
-@testset "Test Sphere(2) prior" begin
+@testset "Test Sphere(2) prior and relative (broken)" begin
 ##
 
 Base.convert(::Type{<:Tuple}, M::Sphere{2, ℝ}) = (:Euclid, :Euclid)
@@ -26,7 +26,7 @@ fg = initfg()
 
 v0 = addVariable!(fg, :x0, Sphere2)
 
-mp = ManifoldPrior(Sphere(2), SA[1., 0, 0], MvNormal([0.01, 0.01]))
+mp = ManifoldPrior(Sphere(2), SA[1., 0, 0], MvNormal([0.01, 0.01]), DefaultOrthonormalBasis(), ExponentialRetraction())
 p = addFactor!(fg, [:x0], mp)
 
 doautoinit!(fg, :x0)