diff --git a/src/Factors/GenericFunctions.jl b/src/Factors/GenericFunctions.jl index f64becd59..00b63039a 100644 --- a/src/Factors/GenericFunctions.jl +++ b/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,21 +99,15 @@ 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 @@ -142,10 +115,8 @@ end # 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 diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl index fd1cf484c..1369cbe4d 100644 --- a/src/IncrementalInference.jl +++ b/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") diff --git a/src/ManifoldSampling.jl b/src/ManifoldSampling.jl new file mode 100644 index 000000000..ceb9b81dd --- /dev/null +++ b/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], ) \ No newline at end of file diff --git a/test/testSpecialEuclidean2Mani.jl b/test/testSpecialEuclidean2Mani.jl index f2adc2ad3..71a07ce8e 100644 --- a/test/testSpecialEuclidean2Mani.jl +++ b/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 ## ====================================================================================== diff --git a/test/testSphereMani.jl b/test/testSphereMani.jl index 5f9a602fc..f4072639e 100644 --- a/test/testSphereMani.jl +++ b/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)