diff --git a/Project.toml b/Project.toml index df7fffb..7162473 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ApproxManifoldProducts" uuid = "9bbbb610-88a1-53cd-9763-118ce10c1f89" keywords = ["mm-iSAM", "SLAM", "inference", "sum-product", "belief-propagation", "nonparametric", "manifolds", "functional"] -version = "0.1.5" +version = "0.2.0" [deps] Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" @@ -13,6 +13,7 @@ KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" LibGit2 = "76f85450-5226-5b5a-8eaa-529ad045b433" Libdl = "8f399da3-3557-5675-b5ff-fb832c97cbdb" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb" Markdown = "d6f4376e-aef5-505a-96c1-9c027394607a" Mmap = "a63ad114-7e13-5084-954f-fe012c677804" NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" @@ -36,6 +37,7 @@ Compat = "1.5.1, 2, 3.2" CoordinateTransformations = "0.5, 0.6, 0.8, 0.9" DocStringExtensions = "0.7, 0.8, 0.9" KernelDensityEstimate = "0.5.3" +ManifoldsBase = "0.10" NLsolve = "3, 4" Optim = "0.16, 0.17, 0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 1" Reexport = "0.2, 1.0" diff --git a/src/API.jl b/src/API.jl index a410f85..fd138ba 100644 --- a/src/API.jl +++ b/src/API.jl @@ -31,14 +31,15 @@ plot( x=getPoints(pq)[1,:], y=getPoints(pq)[2,:], Geom.histogram2d ) """ function manifoldProduct( ff::Vector{BallTreeDensity}, manif::T; - Niter=1, + makeCopy::Bool=false, + Niter::Int=1, addEntropy::Bool=true, recordLabels::Bool=false, selectedLabels::Vector{Vector{Int}}=Vector{Vector{Int}}()) where {T <: Tuple} # # check quick exit if 1 == length(ff) - return ff[1] + return (makeCopy ? x->deepcopy(x) : x->x)(ff[1]) end ndims = Ndim(ff[1]) @@ -55,7 +56,7 @@ function manifoldProduct( ff::Vector{BallTreeDensity}, glbs.recordChoosen = recordLabels pGM, = prodAppxMSGibbsS(dummy, ff, - nothing, nothing, Niter=1, + nothing, nothing, Niter=Niter, addop=addopT, diffop=diffopT, getMu=getManiMu, diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index 31c5e69..6e64a03 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -3,6 +3,9 @@ module ApproxManifoldProducts using Reexport @reexport using KernelDensityEstimate @reexport using TransformUtils +import ManifoldsBase + +const MB = ManifoldsBase using DocStringExtensions @@ -17,6 +20,7 @@ import Base: *, isapprox # import KernelDensityEstimate: kde! import LinearAlgebra: rotate! + const AMP = ApproxManifoldProducts const KDE = KernelDensityEstimate const TUs = TransformUtils @@ -25,7 +29,8 @@ const CTs = CoordinateTransformations # TODO temporary for initial version of on-manifold products KDE.setForceEvalDirect!(true) -export +export + # new local features AMP, get2DLambda, get2DMu, @@ -46,7 +51,6 @@ export manikde!, # general manifolds - Manifold, Circular # internal features not exported diff --git a/src/Circular.jl b/src/Circular.jl index d849429..ef87c10 100644 --- a/src/Circular.jl +++ b/src/Circular.jl @@ -4,21 +4,21 @@ -struct Circular <: Manifold +struct Circular <: MB.Manifold{MB.ℝ} dof::Int addop::Function diffop::Function getMu getLambda domain::Tuple{Float64, Float64} - Circular() = new(1, - addtheta, - difftheta, - getCircMu, - getCircLambda, - (-pi+0.0,pi-1e-15)) end +Circular() = Circular(1, + addtheta, + difftheta, + getCircMu, + getCircLambda, + (-pi+0.0,pi-1e-15)) diff --git a/src/Euclidean.jl b/src/Euclidean.jl index 4bdc23b..0c107e0 100644 --- a/src/Euclidean.jl +++ b/src/Euclidean.jl @@ -1,31 +1,28 @@ # Euclidean Manifold definitions and arithmetic -struct Euclid2 <: Manifold +struct Euclid2 <: MB.Manifold{MB.ℝ} dof::Int addop::Function diffop::Function getMu getLambda domain::Tuple{Tuple{Float64,Float64},Tuple{Float64, Float64}} - Euclid2() = new(2, +, -, KDE.getEuclidMu, KDE.getEuclidLambda, ((-Inf,Inf),(-Inf,Inf))) end +Euclid2() = Euclid2(2, +, -, KDE.getEuclidMu, KDE.getEuclidLambda, ((-Inf,Inf),(-Inf,Inf))) + # ?? -struct EuclideanManifold <: Manifold +struct EuclideanManifold <: MB.Manifold{MB.ℝ} end -get2DLambda(Lambdas::Vector{Float64})::Float64 = sum(Lambdas) - +get2DLambda(Lambdas::AbstractVector{<:Real}) = sum(Lambdas) -function *(PP::Vector{MB{EuclideanManifold,BallTreeDensity}}) - bds = Vector{BallTreeDensity}(undef, length(PP)) - for p in PP - bds[i] = p.belief - end +function *(PP::Vector{MB_{EuclideanManifold,BallTreeDensity}}) + bds = (p->p.belief).(PP) *(bds) end diff --git a/src/Interface.jl b/src/Interface.jl index bdc264a..6bd93ef 100644 --- a/src/Interface.jl +++ b/src/Interface.jl @@ -1,18 +1,14 @@ # Interface - -abstract type Manifold end - - -mutable struct ManifoldBelief{M <: Manifold, B} +mutable struct ManifoldBelief{M <: MB.Manifold{MB.ℝ}, B} manifold::Type{M} belief::B end -ManifoldBelief(m::M,b::B) where {M <: Manifold, B} = ManifoldBelief{M,B} +ManifoldBelief(m::M,b::B) where {M <: MB.Manifold{MB.ℝ}, B} = ManifoldBelief{M,B} -const MB{M,B} = ManifoldBelief{M, B} +const MB_{M,B} = ManifoldBelief{M, B} -function *(PP::Vector{MB{M,B}}) where {M<:Manifold,B} +function *(PP::Vector{MB_{M,B}}) where {M<:MB.Manifold{MB.ℝ},B} @info "taking manifold product of $(length(PP)) terms, $M, $B" @error "No known product definition" end diff --git a/src/KernelHilbertEmbeddings.jl b/src/KernelHilbertEmbeddings.jl index 691977e..c77d35d 100644 --- a/src/KernelHilbertEmbeddings.jl +++ b/src/KernelHilbertEmbeddings.jl @@ -16,17 +16,17 @@ export # abstract type ManifoldDefs end # # FIXME standardize with Manifolds.jl -struct Euclid <: Manifold end -struct SE2_Manifold <: Manifold end -struct SE3_Manifold <: Manifold end - -function ker(::Type{Euclid}, - x::AbstractArray{<:Real,2}, - y::AbstractArray{<:Real,2}, - dx::Vector{<:Real}, - i::Int, - j::Int; - sigma::Real=0.001 ) +struct Euclid <: MB.Manifold{MB.ℝ} end +struct SE2_Manifold <: MB.Manifold{MB.ℝ} end +struct SE3_Manifold <: MB.Manifold{MB.ℝ} end + +function ker( ::Type{Euclid}, + x::AbstractArray{<:Real,2}, + y::AbstractArray{<:Real,2}, + dx::Vector{<:Real}, + i::Int, + j::Int; + sigma::Real=0.001 ) # dx[1] = x[1,i] dx[1] -= y[1,j] @@ -35,13 +35,13 @@ function ker(::Type{Euclid}, exp( dx[1] ) end -function ker(::Type{Euclid2}, - x::AbstractArray{<:Real,2}, - y::AbstractArray{<:Real,2}, - dx::Vector{<:Real}, - i::Int, - j::Int; - sigma::Real=0.001 ) +function ker( ::Type{Euclid2}, + x::AbstractArray{<:Real,2}, + y::AbstractArray{<:Real,2}, + dx::Vector{<:Real}, + i::Int, + j::Int; + sigma::Real=0.001 ) # dx[1] = x[1,i] dx[2] = x[2,i] @@ -53,13 +53,13 @@ function ker(::Type{Euclid2}, exp( dx[1] ) end -function ker(::Type{SE2_Manifold}, - x::AbstractMatrix{<:Real}, - y::AbstractMatrix{<:Real}, - dx::Vector{<:Real}, - i::Int, - j::Int; - sigma::Real=0.001 ) +function ker( ::Type{SE2_Manifold}, + x::AbstractMatrix{<:Real}, + y::AbstractMatrix{<:Real}, + dx::Vector{<:Real}, + i::Int, + j::Int; + sigma::Real=0.001 ) # innov = se2vee(SE2(x[:,i])\SE2(y[:,j])) exp( -sigma*( innov'*innov ) ) @@ -67,13 +67,13 @@ end # This functin is still very slow, needs speedup # Obviously want to get away from the Euler angles throughout -function ker(::Type{SE3_Manifold}, - x::AbstractMatrix{<:Real}, - y::AbstractMatrix{<:Real}, - dx::Vector{<:Real}, - i::Int, - j::Int; - sigma::Real=0.001 ) +function ker( ::Type{SE3_Manifold}, + x::AbstractMatrix{<:Real}, + y::AbstractMatrix{<:Real}, + dx::Vector{<:Real}, + i::Int, + j::Int; + sigma::Real=0.001 ) # innov = veeEuler(SE3(x[1:3,i],Euler((x[4:6,i])...))\SE3(y[1:3,j],Euler((y[4:6,j])...))) exp( -sigma*( innov'*innov ) ) @@ -89,7 +89,7 @@ Notes: - This is the in-place version (well more in-place than mmd) DevNotes: -- TODO make work for different sizes +- TODO make work for different sizes N,M - TODO dont assume equally weighted particles Related @@ -99,8 +99,9 @@ mmd, ker function mmd!(val::AbstractVector{<:Real}, a::AbstractArray{<:Real,2}, b::AbstractArray{<:Real,2}, - mani::Type{<:Manifold}=Euclid, - N::Int=size(a,2), M::Int=size(b,2); bw::Vector{<:Real}=[0.001;] ) + mani::Type{<: MB.Manifold}=Euclid, + N::Int=size(a,2), M::Int=size(b,2); + bw::AbstractVector{<:Real}=[0.001;] ) # # TODO allow unequal data too @assert N == M @@ -108,9 +109,9 @@ function mmd!(val::AbstractVector{<:Real}, val[1] = 0.0 dx = zeros(2) @inbounds @fastmath for i in 1:N - @simd for j in 1:M - val[1] -= ker(mani, a, b, dx, i, j, sigma=bw[1]) - end + @simd for j in 1:M + val[1] -= ker(mani, a, b, dx, i, j, sigma=bw[1]) + end end val .*= 2.0 @inbounds @fastmath for i in 1:N @@ -136,15 +137,16 @@ Related mmd!, ker """ -function mmd(a::AbstractArray{<:Real,2}, - b::AbstractArray{<:Real,2}, - mani::Type{<:Manifold}=Euclid, - N::Int=size(a,2), M::Int=size(b,2); bw::Vector{<:Real}=[0.001;]) +function mmd( a::AbstractArray{<:Real,2}, + b::AbstractArray{<:Real,2}, + mani::Type{<: MB.Manifold}=Euclid, + N::Int=size(a,2), M::Int=size(b,2); + bw::AbstractVector{<:Real}=[0.001;]) # val = [0.0;] - mmd!(val, a,b, - mani, - N, M; bw=bw ) + mmd!( val, a,b, + mani, + N, M; bw=bw ) # return val[1] end