diff --git a/Project.toml b/Project.toml index c000181..5a0aa9e 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ApproxManifoldProducts" uuid = "9bbbb610-88a1-53cd-9763-118ce10c1f89" keywords = ["MM-iSAMv2", "SLAM", "inference", "sum-product", "belief-propagation", "nonparametric", "manifolds", "functional"] -version = "0.4.16" +version = "0.4.17" [deps] Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" @@ -34,13 +34,13 @@ UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" Unicode = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" [compat] -CoordinateTransformations = "0.5, 0.6, 0.8, 0.9" +CoordinateTransformations = "0.5, 0.6" DocStringExtensions = "0.7, 0.8, 0.9" -KernelDensityEstimate = "0.5.6" +KernelDensityEstimate = "0.5.9" Manifolds = "0.6" ManifoldsBase = "0.11, 0.12" NLsolve = "3, 4" -Optim = "0.16, 0.17, 0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 1" +Optim = "1" Reexport = "0.2, 1.0" Requires = "0.5, 1" SLEEFPirates = "0.5, 0.6" @@ -51,7 +51,9 @@ julia = "1.4" [extras] DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0" +FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549" +JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [targets] -test = ["DataFrames", "Test"] +test = ["DataFrames", "FileIO", "JLD2", "Test"] diff --git a/src/API.jl b/src/API.jl index 5d452d7..2b8cce4 100644 --- a/src/API.jl +++ b/src/API.jl @@ -42,6 +42,8 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity}, addEntropy::Bool=true, recordLabels::Bool=false, selectedLabels::Vector{Vector{Int}}=Vector{Vector{Int}}(), + _randU=Vector{Float64}(), + _randN=Vector{Float64}(), logger=ConsoleLogger() ) where {M <: MB.AbstractManifold, P} # # check quick exit @@ -87,7 +89,21 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity}, end end - # @assert !isPartial(ff[1]) "currently only supports first product element as full density, isPartial.=$(isPartial.(ff)), dims.=$(Ndim.(_ff))" + ndims = maximum(Ndim.(_ff)) + Ndens = length(_ff) + Np = Npts(inplace) + maxNp = maximum(Int[Np; Npts.(_ff)]) + Nlevels = floor(Int,(log(Float64(maxNp))/log(2.0))+1.0) + if 0 == length(_randU) + _len = Int(Np*Ndens*(Niter+2)*Nlevels) + resize!(_randU, _len) + _randU .= rand(_len) + end + if 0 == length(_randN) + _len = Int(ndims*Np*(Nlevels+1)) + resize!(_randN, _len) + _randN .= randn(_len) + end ## TODO check both _ff and inplace use a matrix of coordinates (columns) # expects Matrix with columns as samples and rows are coordinate dimensions @@ -98,7 +114,14 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity}, diffop=diffopT, getMu=getManiMu, glbs=glbs, - addEntropy=addEntropy ); + addEntropy=addEntropy, + ndims=ndims, + Ndens=Ndens, + Np=Np, + maxNp=maxNp, + Nlevels=Nlevels, + randU=_randU, + randN=_randN ); # if recordLabels diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index dc4d982..57df306 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -26,6 +26,7 @@ using Statistics import Base: *, isapprox, convert import LinearAlgebra: rotate! import Statistics: mean +import KernelDensityEstimate: getPoints, getBW const AMP = ApproxManifoldProducts diff --git a/src/Legacy.jl b/src/Legacy.jl index 316e825..36edb19 100644 --- a/src/Legacy.jl +++ b/src/Legacy.jl @@ -101,11 +101,10 @@ end # getManifold(x::ManifoldKernelDensity) = x.manifold -import KernelDensityEstimate: getPoints, getBW, Ndim, Npts, getWeights, marginal +import KernelDensityEstimate: Ndim, Npts, getWeights, marginal import KernelDensityEstimate: getKDERange, getKDEMax, getKDEMean, getKDEfit import KernelDensityEstimate: sample, rand, resample, kld, minkld -getBW(x::ManifoldKernelDensity, w...;kw...) = getBW(x.belief,w...;kw...) Ndim(x::ManifoldKernelDensity, w...;kw...) = Ndim(x.belief,w...;kw...) Npts(x::ManifoldKernelDensity, w...;kw...) = Npts(x.belief,w...;kw...) diff --git a/src/entities/ManifoldKernelDensity.jl b/src/entities/ManifoldKernelDensity.jl index e997967..81c4c63 100644 --- a/src/entities/ManifoldKernelDensity.jl +++ b/src/entities/ManifoldKernelDensity.jl @@ -1,6 +1,6 @@ -struct ManifoldKernelDensity{M <: MB.AbstractManifold{MB.ℝ}, B <: BallTreeDensity, L, P} +struct ManifoldKernelDensity{M <: MB.AbstractManifold, B <: BallTreeDensity, L, P} manifold::M """ legacy expects matrix of coordinates (as columns) """ belief::B diff --git a/src/services/ManifoldKernelDensity.jl b/src/services/ManifoldKernelDensity.jl index 5855778..24f72f8 100644 --- a/src/services/ManifoldKernelDensity.jl +++ b/src/services/ManifoldKernelDensity.jl @@ -104,50 +104,6 @@ Alias for overloaded `Statistics.mean`. calcMean(mkd::ManifoldKernelDensity, aspartial::Bool=true) = mean(mkd, aspartial) - -# internal workaround function for building partial submanifold dimensions, must be upgraded/standarized -function _buildManifoldPartial( fullM::MB.AbstractManifold, - partial_coord_dims ) - # - # temporary workaround during Manifolds.jl integration - manif = convert(Tuple, fullM)[partial_coord_dims] - # - newMani = MB.AbstractManifold[] - for me in manif - push!(newMani, _reducePartialManifoldElements(me)) - end - - # assume independent dimensions for definition, ONLY USED AS DECORATOR AT THIS TIME, FIXME - return ProductManifold(newMani...) -end - -""" - $SIGNATURES - -Return true if this ManifoldKernelDensity is a partial. -""" -isPartial(mkd::ManifoldKernelDensity{M,B,L}) where {M,B,L} = true -isPartial(mkd::ManifoldKernelDensity{M,B,Nothing}) where {M,B} = false - -# override -function marginal(x::ManifoldKernelDensity{M,B}, - dims::AbstractVector{<:Integer} ) where {M <: AbstractManifold , B} - # - ldims::Vector{Int} = collect(dims) - ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0) -end - -function marginal(x::ManifoldKernelDensity{M,B,L}, - dims::AbstractVector{<:Integer} ) where {M <: AbstractManifold , B, L <: AbstractVector{<:Integer}} - # - # @show dims x._partial - ldims::Vector{Int} = intersect(x._partial, dims) - ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0) -end -# manis = convert(Tuple, x.manifold) -# partMani = _reducePartialManifoldElements(manis[dims]) -# pts = getPoints(x) - _getFieldPartials(mkd::ManifoldKernelDensity{M,B,Nothing}, field::Function, aspartial::Bool=true) where {M,B} = field(mkd) function _getFieldPartials(mkd::ManifoldKernelDensity{M,B,<:AbstractVector}, field::Function, aspartial::Bool=true) where {M,B} @@ -167,34 +123,37 @@ function _getFieldPartials(mkd::ManifoldKernelDensity{M,B,<:AbstractVector}, fie end end + + getInfoPerCoord(mkd::ManifoldKernelDensity, aspartial::Bool=true) = _getFieldPartials(mkd, x->x.infoPerCoord, aspartial) getBandwidth(mkd::ManifoldKernelDensity, aspartial::Bool=true) = _getFieldPartials(mkd, x->getBW(x)[:,1], aspartial) -function antimarginal(newM::AbstractManifold, - u0, - mkd::ManifoldKernelDensity, - newpartial::AbstractVector{<:Integer} ) +# internal workaround function for building partial submanifold dimensions, must be upgraded/standarized +function _buildManifoldPartial( fullM::MB.AbstractManifold, + partial_coord_dims ) # - - # convert to antimarginal by copying user provided example point for bigger manifold - pts = getPoints(mkd, false) - nPts = Vector{typeof(u0)}(undef, length(pts)) - for (i,pt) in enumerate(pts) - nPts[i] = setPointPartial!(newM, deepcopy(u0), mkd.manifold, pt, newpartial) + # temporary workaround during Manifolds.jl integration + manif = convert(Tuple, fullM)[partial_coord_dims] + # + newMani = MB.AbstractManifold[] + for me in manif + push!(newMani, _reducePartialManifoldElements(me)) end - # also update metadata elements - finalpartial = !isPartial(mkd) ? newpartial : error("not built yet, to shift incoming partial") - bw = zeros(manifold_dimension(newM)) - bw[finalpartial] .= getBW(mkd)[:,1] - ipc = zeros(manifold_dimension(newM)) - ipc[finalpartial] .= getInfoPerCoord(mkd, true) - - manikde!(newM, nPts, u0, bw=bw, partial=finalpartial, infoPerCoord=ipc) + # assume independent dimensions for definition, ONLY USED AS DECORATOR AT THIS TIME, FIXME + return ProductManifold(newMani...) end +""" + $SIGNATURES + +Return true if this ManifoldKernelDensity is a partial. +""" +isPartial(mkd::ManifoldKernelDensity{M,B,L}) where {M,B,L} = true +isPartial(mkd::ManifoldKernelDensity{M,B,Nothing}) where {M,B} = false + """ $SIGNATURES @@ -227,6 +186,15 @@ function getPoints( x::ManifoldKernelDensity{M,B,L}, end +function getBW(x::ManifoldKernelDensity{M,B,L}, asPartial::Bool=true; kw...) where {M,B,L} + bw = getBW(x.belief; kw...) + if L !== Nothing && asPartial + return view(bw, x._partial) + end + return bw +end + + # TODO check that partials / marginals are sampled correctly function sample(x::ManifoldKernelDensity{M,B,L,P}, N::Int) where {M,B,L,P} # get legacy matrix of coordinates and selected labels @@ -285,9 +253,56 @@ function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P} nothing end + Base.show(io::IO, ::MIME"text/plain", mkd::ManifoldKernelDensity) = show(io, mkd) Base.show(io::IO, ::MIME"application/juno.inline", mkd::ManifoldKernelDensity) = show(io, mkd) + +# override +function marginal(x::ManifoldKernelDensity{M,B}, + dims::AbstractVector{<:Integer} ) where {M <: AbstractManifold , B} + # + ldims::Vector{Int} = collect(dims) + ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0) +end + +function marginal(x::ManifoldKernelDensity{M,B,L}, + dims::AbstractVector{<:Integer} ) where {M <: AbstractManifold , B, L <: AbstractVector{<:Integer}} + # + # @show dims x._partial + ldims::Vector{Int} = intersect(x._partial, dims) + ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0) +end +# manis = convert(Tuple, x.manifold) +# partMani = _reducePartialManifoldElements(manis[dims]) +# pts = getPoints(x) + + + +function antimarginal(newM::AbstractManifold, + u0, + mkd::ManifoldKernelDensity, + newpartial::AbstractVector{<:Integer} ) + # + + # convert to antimarginal by copying user provided example point for bigger manifold + pts = getPoints(mkd, false) + nPts = Vector{typeof(u0)}(undef, length(pts)) + for (i,pt) in enumerate(pts) + nPts[i] = setPointPartial!(newM, deepcopy(u0), mkd.manifold, pt, newpartial) + end + + # also update metadata elements + finalpartial = !isPartial(mkd) ? newpartial : error("not built yet, to shift incoming partial") + bw = zeros(manifold_dimension(newM)) + bw[finalpartial] .= getBW(mkd)[:,1] + ipc = zeros(manifold_dimension(newM)) + ipc[finalpartial] .= getInfoPerCoord(mkd, true) + + manikde!(newM, nPts, u0, bw=bw, partial=finalpartial, infoPerCoord=ipc) +end + + ## ======================================================================================= ## deprecate as necessary below ## ======================================================================================= diff --git a/src/services/ManifoldPartials.jl b/src/services/ManifoldPartials.jl index a83ff77..429c52f 100644 --- a/src/services/ManifoldPartials.jl +++ b/src/services/ManifoldPartials.jl @@ -204,9 +204,9 @@ function getManifoldPartial(M::ProductManifold, end function getManifoldPartial(M::GroupManifold, - partial::AbstractVector{Int}, + partial::AbstractVector{<:Integer}, repr::_PartiableRepresentation=nothing, - offset::Base.RefValue{Int}=Ref(0); + offset::Base.RefValue{<:Integer}=Ref(0); doError::Bool=true ) # # mask the desired coordinate dimensions diff --git a/test/runtests.jl b/test/runtests.jl index 67965fa..f0fdb29 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -11,6 +11,7 @@ include("testManiProductBigSmall.jl") include("testBasicManiProduct.jl") include("testMarginalProducts.jl") include("testMMD.jl") +include("testPartialProductSE2.jl") include("basic_se3.jl") # diff --git a/test/testManifoldPartial.jl b/test/testManifoldPartial.jl index 130a673..caca1f8 100644 --- a/test/testManifoldPartial.jl +++ b/test/testManifoldPartial.jl @@ -187,8 +187,8 @@ X = manikde!(M, pts, partial=[1;3]) X_ = replace(X0, X) # check metadata -@test isapprox( getBW(X_)[[1;3],1], getBW(X)[[1;3],1] ) -@test !isapprox( getBW(X_)[[1;3],1], getBW(X0)[[1;3],1] ) +@test isapprox( getBW(X_, false)[[1;3],1], getBW(X, false)[[1;3],1] ) +@test !isapprox( getBW(X_, false)[[1;3],1], getBW(X0, false)[[1;3],1] ) @test isapprox( X_.infoPerCoord[[1;3]], X.infoPerCoord[[1;3]] ) diff --git a/test/testMarginalProducts.jl b/test/testMarginalProducts.jl index 5df20cd..9cddfbe 100644 --- a/test/testMarginalProducts.jl +++ b/test/testMarginalProducts.jl @@ -108,8 +108,8 @@ P12_ for sidx in 1:N - bw1 = getBW(P1)[:,1] .^2 - bw2 = getBW(P2_)[:,1] .^2 + bw1 = getBW(P1, false)[:,1] .^2 + bw2 = getBW(P2_, false)[:,1] .^2 u1 = pts1[sl[sidx][1]] u2 = pts2[sl[sidx][2]] @@ -165,9 +165,9 @@ P123_ for sidx in 1:N - bw1 = getBW(P1)[:,1] .^2 - bw2 = getBW(P2_)[:,1] .^2 - bw3 = getBW(P3_)[:,1] .^2 + bw1 = getBW(P1, false)[:,1] .^2 + bw2 = getBW(P2_, false)[:,1] .^2 + bw3 = getBW(P3_, false)[:,1] .^2 u1 = pts1[sl[sidx][1]] u2 = pts2[sl[sidx][2]] @@ -224,9 +224,9 @@ pts = getPoints(P) for sidx in 1:N - bw1 = getBW(P1)[:,1] .^2 - bw2 = getBW(P2)[:,1] .^2 - bw3 = getBW(P3)[:,1] .^2 + bw1 = getBW(P1, false)[:,1] .^2 + bw2 = getBW(P2, false)[:,1] .^2 + bw3 = getBW(P3, false)[:,1] .^2 # full density first u2 = pts2[sl[sidx][1]] @@ -282,8 +282,8 @@ pts = getPoints(P_) for sidx in 1:N - bw1 = getBW(P1)[:,1] .^2 - bw3 = getBW(P3)[:,1] .^2 + bw1 = getBW(P1, false)[:,1] .^2 + bw3 = getBW(P3, false)[:,1] .^2 u1 = pts1[sl[sidx][1]] u3 = pts3[sl[sidx][2]] @@ -343,10 +343,10 @@ P45__ # sidx = 1 for sidx in 1:N - bw1 = getBW(P4)[:,1] .^2 - bw2 = getBW(P4_)[:,1] .^2 - bw3 = getBW(P5)[:,1] .^2 - bw4 = getBW(P5_)[:,1] .^2 + bw1 = getBW(P4, false)[:,1] .^2 + bw2 = getBW(P4_, false)[:,1] .^2 + bw3 = getBW(P5, false)[:,1] .^2 + bw4 = getBW(P5_, false)[:,1] .^2 u1 = pts4[ sl[sidx][1]] u2 = pts4_[sl[sidx][2]] @@ -410,9 +410,9 @@ pts = getPoints(P) for sidx in 1:N - bw1 = getBW(P1)[:,1] .^2 - bw2 = getBW(P2)[:,1] .^2 - bw3 = getBW(P3)[:,1] .^2 + bw1 = getBW(P1, false)[:,1] .^2 + bw2 = getBW(P2, false)[:,1] .^2 + bw3 = getBW(P3, false)[:,1] .^2 # full density first u2 = pts2[sl[sidx][1]] @@ -472,9 +472,9 @@ pts = getPoints(P) for sidx in 1:N - bw1 = getBW(P1)[:,1] .^2 - bw2 = getBW(P2)[:,1] .^2 - bw3 = getBW(P3)[:,1] .^2 + bw1 = getBW(P1, false)[:,1] .^2 + bw2 = getBW(P2, false)[:,1] .^2 + bw3 = getBW(P3, false)[:,1] .^2 # full density first u1 = pts1[sl[sidx][1]] diff --git a/test/testPartialProductSE2.jl b/test/testPartialProductSE2.jl new file mode 100644 index 0000000..2e02a3f --- /dev/null +++ b/test/testPartialProductSE2.jl @@ -0,0 +1,157 @@ +# test on partial products with SpecialEuclidean(2) + +using Manifolds +using ApproxManifoldProducts +using Test +# using Random +using FileIO + +## + +@testset "partial product with a SpecialEuclidean(2)" begin +## + +datafile = joinpath(@__DIR__, "testdata", "partialtest.jld2") +dict = load(datafile) +pts1 = dict["pts1"] +pts2 = dict["pts2"] + +randU = Float64[] +randN = Float64[] + +len = length(pts1) + +# define test manifold +M = SpecialEuclidean(2) +e0 = identity_element(M) + +# p1 full SpecialEuclidean(2) +p1 = manikde!(M, pts1) +p1_ = marginal(p1,[1;2]) + +# p2 only Translation(2) part +_p2 = manikde!(M, pts2) +p2 = marginal(_p2, [1;2]) + +# product of full and marginal +# p12 = p1*p2 + +## =======================FIRST PRODUCT================================================ + +selectedLabels=Vector{Vector{Int}}() +p12 = manifoldProduct([p1; p2]; addEntropy=false, + recordLabels=true, + selectedLabels=selectedLabels, + _randU=randU, + _randN=randN ) +# +selectedLabels +# + +@test !isPartial(p12) + +p12_ = marginal(p12, [1;2]) +@test isPartial(p12_) + +_p12_ = manikde!(TranslationGroup(2), getPoints(p12_)) + +## intermediate test, check product of selected kernels match what is in the marginal + +for sidx = 1:len + bw1 = getBW(p1)[:,1] .^2 + bw2 = getBW(p2, false)[:,1] .^2 + + u1 = pts1[selectedLabels[sidx][1]] + u2 = pts2[selectedLabels[sidx][2]] + + u12, = calcProductGaussians(M, [u1,u2], [bw1,bw2]); + + @test isapprox( u12.parts[1], getPoints(p12)[sidx].parts[1] ) +end + +## now check the marginal dimensions only + +# now do submanifold dimensions separately as reference test -- should get a similar result +pts1_ = getPoints(p1_) +pts2_ = getPoints(p2) + +## Do the translation part separate + +for sidx = 1:len + bw1 = getBW(p1_)[:,1] .^2 + bw2 = getBW(p2)[:,1] .^2 + + u1 = pts1_[selectedLabels[sidx][1]] + u2 = pts2_[selectedLabels[sidx][2]] + + u12, = calcProductGaussians(TranslationGroup(2), [u1,u2], [bw1,bw2]) + + @test isapprox( u12, getPoints(p12)[sidx].parts[1] ) +end + + + +## =======================SECOND PRODUCT================================================ +## NEW PRODUCT ON ONLY THE TRANSLATION PART, COMPARE WITH ABOVE RESULT + + +p1__ = manikde!(TranslationGroup(2), pts1_) +p2__ = manikde!(TranslationGroup(2), pts2_) + +# p12__ = p1__*p2__ +selectedLabels__=Vector{Vector{Int}}() +p12__ = manifoldProduct([p1__; p2__]; addEntropy=false, + recordLabels=true, + selectedLabels=selectedLabels__, + _randU=randU, + _randN=randN ) +# +selectedLabels__ + + +## compare calcProduct of full*partial with selection from partial*partial + +sidx = 1 +for sidx = 1:len + +bw1 = getBW(p1)[:,1] .^2 +bw2 = getBW(p2, false)[:,1] .^2 + +# full-partial points, but selected from partial-partial product +u1 = pts1[selectedLabels__[sidx][1]] +u2 = pts2[selectedLabels__[sidx][2]] + +u12, = calcProductGaussians(M, [u1,u2], [bw1,bw2]); +u12_, = calcProductGaussians(TranslationGroup(2), [u1.parts[1],u2.parts[1]], [bw1[1:2],bw2[1:2]]); + +@test isapprox( u12.parts[1], u12_ ) +@test isapprox( getPoints(p12__)[sidx], u12_ ) + +end + +## + + +@test mmd(_p12_, p12__) < 0.001 + + +## +end + + + +## DEBUG PLOTTING ========================================================================== + +## Plots showing the problem, p12 was wrong!! + +# using Cairo, RoMEPlotting +# Gadfly.set_default_plot_size(35cm,20cm) + +# n=10; plotKDE([p1_;p2; p12], levels=3, selectedPoints=selectedLabels[n:n]) +# n=3; plotKDE([p1__;p2__; p12__], levels=3, selectedPoints=selectedLabels__[n:n]) +# plotKDE([p1__; p2__; p12__]) + +# plotKDE([_p12_; p12__]) + + +## \ No newline at end of file diff --git a/test/testdata/partialtest.jld2 b/test/testdata/partialtest.jld2 new file mode 100644 index 0000000..67ff293 Binary files /dev/null and b/test/testdata/partialtest.jld2 differ