From 843e2fdcdb47ef4f6b2a811d4fc9395ea2e72356 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 21 Jan 2024 01:49:08 -0800 Subject: [PATCH 01/31] compat stats v1 --- Project.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/Project.toml b/Project.toml index 73af05a..5546e42 100644 --- a/Project.toml +++ b/Project.toml @@ -51,6 +51,7 @@ Reexport = "0.2, 1.0" Requires = "0.5, 1" Rotations = "1" StaticArrays = "0.15, 1" +Statistics = "1" TensorCast = "0.2, 0.3, 0.4" TransformUtils = "0.2.10" julia = "1.9" From de71630d5c9681810a77bbe6ed852a0345b458a1 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 22 Jan 2024 05:48:40 -0800 Subject: [PATCH 02/31] clean circular code formating --- src/services/CircularUtils.jl | 125 +++++++++++++++++++--------------- 1 file changed, 71 insertions(+), 54 deletions(-) diff --git a/src/services/CircularUtils.jl b/src/services/CircularUtils.jl index a0b9d94..966e071 100644 --- a/src/services/CircularUtils.jl +++ b/src/services/CircularUtils.jl @@ -28,7 +28,7 @@ difftheta(wth1, wth2) = log(_AMP_CIRCLE, wth2, wth1) # logmap_SO2(TUs.R(wth1)'*T addtheta(wth1, wth2) = exp(_AMP_CIRCLE, wth2, wth1) # TUs.wrapRad( wth1+wth2 ) # manifold get Gaussian products mean -getCircMu(m::Vector{Float64}, s::Vector{Float64}, dummy::Float64) = addtheta(0, get2DMu(m, s, diffop=difftheta, initrange=(-pi+0.0,pi+0.0)) ) +getCircMu(m::Vector{Float64}, s::Vector{Float64}, ::Float64) = addtheta(0, get2DMu(m, s, diffop=difftheta, initrange=(-pi+0.0,pi+0.0)) ) # getCircMu = (m::Vector{Float64}, s::Vector{Float64}, dummy::Float64) -> TUs.wrapRad(get2DMuMin(m, s, diffop=difftheta, initrange=(-pi+0.0,pi+0.0))) @@ -43,32 +43,38 @@ Probability density function `p(x)`, as estimated by kernels hatp_{-j}(x) = 1/(N-1) Σ_{i != j}^N frac{1}{sqrt{2pi}σ } exp{ -frac{(x-μ)^2}{2 σ^2} } ``` """ -function normDistAccAt!(ret::AV, - idx::Int, - x::Float64, - sigma::Float64, - w::Float64=1.0 )::Nothing where {AV <: AbstractVector} +function normDistAccAt!( + ret::AV, + idx::Int, + x::Float64, + sigma::Float64, + w::Float64=1.0 +) where {AV <: AbstractVector} global reci_s2pi @fastmath ret[idx] += w*reci_s2pi/sigma * exp( - (x^2)/(2.0*(sigma^2)) ) return nothing end -function rbfAccAt!( ret::AV, - idx::Int, - x::Float64, - μ::Float64=0.0, - σ::Float64=1.0, - w::Float64=1.0, - diffop::Function=-)::Nothing where {AV <: AbstractVector} +function rbfAccAt!( + ret::AV, + idx::Int, + x::Float64, + μ::Float64=0.0, + σ::Float64=1.0, + w::Float64=1.0, + diffop::Function=- +) where {AV <: AbstractVector} # normDistAccAt!(ret, idx, diffop(x, μ), σ, w) nothing end -function rbf!( ret::AV, - x::Float64, - μ::Float64=0.0, - σ::Float64=1.0, - diffop::Function=-)::Nothing where {AV <: AbstractVector} +function rbf!( + ret::AV, + x::Float64, + μ::Float64=0.0, + σ::Float64=1.0, + diffop::Function=- +) where {AV <: AbstractVector} # ret[1] = 0.0 normDistAccAt!(ret, 1, diffop(x,μ), σ) @@ -76,7 +82,11 @@ function rbf!( ret::AV, end -function rbf(x::Float64, μ::Float64=0.0, σ::Float64=1.0) +function rbf( + x::Float64, + μ::Float64=0.0, + σ::Float64=1.0 +) ret = Vector{Float64}(undef, 1) # initialized in rbf!(..) rbf!(ret, x, μ, σ) return ret[1] @@ -89,13 +99,15 @@ end Evalute the KDE naively as equally weighted Gaussian kernels with common bandwidth. This function does, however, allow on-manifold evaluations. """ -function evaluateManifoldNaive1D!( ret::Vector{Float64}, - idx::Int, - pts::Array{Float64,1}, - bw::Float64, - x::Array{Float64,1}, - loo::Int=-1, - diffop=- )::Nothing +function evaluateManifoldNaive1D!( + ret::Vector{Float64}, + idx::Int, + pts::Array{Float64,1}, + bw::Float64, + x::Array{Float64,1}, + loo::Int=-1, + diffop=- +) # dontskip = loo == -1 N = length(pts) @@ -109,12 +121,14 @@ function evaluateManifoldNaive1D!( ret::Vector{Float64}, return nothing end -function evaluateManifoldNaive1D!( ret::Vector{Float64}, - idx::Int, - bd::BallTreeDensity, - x::Array{Float64,1}, - loo::Int=-1, - diffop=- )::Nothing +function evaluateManifoldNaive1D!( + ret::Vector{Float64}, + idx::Int, + bd::BallTreeDensity, + x::Array{Float64,1}, + loo::Int=-1, + diffop=- +) # evaluateManifoldNaive1D!(ret, idx, getPoints(bd)[:], getBW(bd)[1,1], x, loo, diffop ) end @@ -143,10 +157,12 @@ This quantity `CV` is related to an entropy `H(p)` estimate via: H(p) = -CV(p) ``` """ -function manifoldLooCrossValidation(pts::Array, - bw::Float64; - own::Bool=true, - diffop::Function=- ) +function manifoldLooCrossValidation( + pts::Array, + bw::Float64; + own::Bool=true, + diffop::Function=- +) # N = maximum(size(pts)) h = [bw;] @@ -168,26 +184,27 @@ function manifoldLooCrossValidation(pts::Array, end -function kde!_CircularNaiveCV(points::A) where {A <: AbstractArray{Float64,1}} - - # initial setup parameters - dims = 1 # size(points,1) - bwds = zeros(dims) - # initial testing values - lower = 0.001 - upper = 2pi - - # excessive for loop for leave one out likelihiood cross validation (Silverman 1986, p.52) - for i in 1:dims - minEntropyLOOCV = (bw) -> -manifoldLooCrossValidation(points, bw, own=true, diffop=difftheta) - res = Optim.optimize(minEntropyLOOCV, lower, upper, Optim.GoldenSection(), x_tol=0.001) - bwds[i] = res.minimizer - end +function kde!_CircularNaiveCV( + points::AbstractVector +) + # initial setup parameters + dims = 1 # size(points,1) + bwds = zeros(dims) + # initial testing values + lower = 0.001 + upper = 2pi + + # excessive for loop for leave one out likelihiood cross validation (Silverman 1986, p.52) + for i in 1:dims + minEntropyLOOCV = (bw) -> -manifoldLooCrossValidation(points, bw, own=true, diffop=difftheta) + res = Optim.optimize(minEntropyLOOCV, lower, upper, Optim.GoldenSection(), x_tol=0.001) + bwds[i] = res.minimizer + end - # cosntruct the kde with CV optimized bandwidth - p = kde!( points, bwds, (addtheta,), (difftheta,) ) + # cosntruct the kde with CV optimized bandwidth + p = kde!( points, bwds, (addtheta,), (difftheta,) ) - return p + return p end From f401955335579c3ea9b4851e8d2fb6f0ba12c639 Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Mon, 29 Jan 2024 03:18:04 -0800 Subject: [PATCH 03/31] wire up MKD to build ManellicTree, AMP tests ok (#275) * wire up MKD to build ManellicTree, AMP tests ok * kernel evaluation with bw scale * minor comment to remember manellic bw test --- src/ApproxManifoldProducts.jl | 13 +++++---- src/Legacy.jl | 5 ++++ src/entities/KernelEval.jl | 9 ++++++ src/entities/ManifoldKernelDensity.jl | 3 +- src/services/KernelEval.jl | 8 ----- src/services/ManellicTree.jl | 42 +++++++++++++++++++-------- src/services/ManifoldKernelDensity.jl | 11 +++++-- test/manellic/testManellicTree.jl | 42 ++++++++++++++++++++++++++- 8 files changed, 102 insertions(+), 31 deletions(-) create mode 100644 src/entities/KernelEval.jl diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index 6dd72e0..ef84444 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -48,8 +48,14 @@ include("ExportAPI.jl") # internal features not exported include("_BiMaps.jl") -# AMP types and some legacy support +include("entities/KernelEval.jl") +include("entities/ManellicTree.jl") # experimental include("entities/ManifoldKernelDensity.jl") + +include("CommonUtils.jl") +include("services/ManellicTree.jl") + +# AMP types and some legacy support include("entities/ManifoldDefinitions.jl") include("Legacy.jl") include("services/ManifoldPartials.jl") @@ -57,7 +63,6 @@ include("Interface.jl") # regular features include("services/KernelEval.jl") -include("CommonUtils.jl") include("services/ManifoldKernelDensity.jl") include("services/Euclidean.jl") include("services/CircularUtils.jl") @@ -71,10 +76,6 @@ include("API.jl") include("Deprecated.jl") -# experimental -include("entities/ManellicTree.jl") -include("services/ManellicTree.jl") - # weak dependencies include("../ext/WeakdepsPrototypes.jl") diff --git a/src/Legacy.jl b/src/Legacy.jl index 7d5a563..f8a5c38 100644 --- a/src/Legacy.jl +++ b/src/Legacy.jl @@ -117,6 +117,11 @@ import KernelDensityEstimate: Ndim, Npts, getWeights, marginal import KernelDensityEstimate: getKDERange, getKDEMax, getKDEMean, getKDEfit import KernelDensityEstimate: sample, rand, resample, kld, minkld +Npts(::ManellicTree{M,D,N}) where {M,D,N} = N +Ndim(mt::ManellicTree) = manifold_dimension(mt.manifold) +getBW(mker::MvNormalKernel) = cov(mker) |> collect +getBW(mt::ManellicTree) = getBW.(mt.leaf_kernels) + 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/KernelEval.jl b/src/entities/KernelEval.jl new file mode 100644 index 0000000..6af1c02 --- /dev/null +++ b/src/entities/KernelEval.jl @@ -0,0 +1,9 @@ + + +abstract type AbstractKernel end + +@kwdef struct MvNormalKernel{T,M,iM} <: AbstractKernel + p::MvNormal{T,M} + # TDB might already be covered in p.Σ.chol but having issues with SymPD (not particular to this AMP repo) + sqrt_iΣ::iM = sqrt(inv(p.Σ)) +end diff --git a/src/entities/ManifoldKernelDensity.jl b/src/entities/ManifoldKernelDensity.jl index 1febefc..d62bd65 100644 --- a/src/entities/ManifoldKernelDensity.jl +++ b/src/entities/ManifoldKernelDensity.jl @@ -1,4 +1,5 @@ +const TreeDensity = Union{<:ManellicTree, <: BallTreeDensity} """ $TYPEDEF @@ -12,7 +13,7 @@ Notes DevNotes - WIP AMP issue 41, use generic retractions during manifold products. """ -struct ManifoldKernelDensity{M <: MB.AbstractManifold, B <: BallTreeDensity, L, P <: AbstractArray} +struct ManifoldKernelDensity{M <: MB.AbstractManifold, B <: TreeDensity, L, P <: AbstractArray} manifold::M """ legacy expects matrix of coordinates (as columns) """ belief::B diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index 81e2252..31ed009 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -8,14 +8,6 @@ function projectSymPosDef(c::AbstractMatrix) end -abstract type AbstractKernel end - -@kwdef struct MvNormalKernel{T,M,iM} <: AbstractKernel - p::MvNormal{T,M} - # TDB might already be covered in p.Σ.chol but having issues with SymPD (not particular to this AMP repo) - sqrt_iΣ::iM = sqrt(inv(p.Σ)) -end - Base.eltype(mt::MvNormalKernel) = eltype(mt.p) function MvNormalKernel(m::AbstractVector,c::AbstractArray) diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 2f1c2e9..01b04ce 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -9,6 +9,7 @@ # end # end + getPoints( mt::ManellicTree ) = view(mt.data, mt.permute) @@ -170,6 +171,12 @@ function buildTree_Manellic!( leaf_size = 1 ) where {MT,D,N} # + _legacybw(s::Nothing) = s + _legacybw(s::AbstractMatrix) = s + _legacybw(s::AbstractVector) = diagm(s) + + _kernel_bw = _legacybw(kernel_bw) + if N <= index return mtree end @@ -180,7 +187,7 @@ function buildTree_Manellic!( # according to current index permutation (i.e. sort data as you build the tree) ido = view(mtree.permute, idc) # split the slice of order-permuted data - ax_CCp, mask, knl = splitPointsEigen(M, view(mtree.data, ido); kernel, kernel_bw) + ax_CCp, mask, knl = splitPointsEigen(M, view(mtree.data, ido); kernel, kernel_bw=_kernel_bw) # sort the data as 'small' and 'big' elements either side of the eigen split sml = view(ido, mask) @@ -199,11 +206,11 @@ function buildTree_Manellic!( if leaf_size < npts if lft != mid_idx # recursively call two branches of tree, left - buildTree_Manellic!(mtree, lft, low, mid_idx; kernel, kernel_bw, leaf_size) + buildTree_Manellic!(mtree, lft, low, mid_idx; kernel, kernel_bw=_kernel_bw, leaf_size) end if rgt != high # and right subtree - buildTree_Manellic!(mtree, rgt, mid_idx+1, high; kernel, kernel_bw, leaf_size) + buildTree_Manellic!(mtree, rgt, mid_idx+1, high; kernel, kernel_bw=_kernel_bw, leaf_size) end end @@ -233,11 +240,18 @@ function buildTree_Manellic!( r_PP[1], CV ) |> typeof - lCV = if isnothing(kernel_bw) - CV - else - kernel_bw - end + + _legacybw(s::AbstractMatrix) = s + _legacybw(s::AbstractVector) = diagm(s) + _legacybw(::Nothing) = CV + + lCV = _legacybw(kernel_bw) + # lCV = if isnothing(kernel_bw) + # CV + # else + # kernel_bw + # end + lknlT = kernel( r_PP[1], lCV @@ -282,18 +296,19 @@ end # - Fast kernels # - Parallel transport shortcuts? function evaluate( - mt::ManellicTree{M,D,N}, + mt::ManellicTree{M,D,N,HL}, p, bw_scl::Real = 1 -) where {M,D,N} +) where {M,D,N,HL} dim = manifold_dimension(mt.manifold) sumval = 0.0 # FIXME, brute force for loop for i in 1:N ekr = mt.leaf_kernels[i] - nscl = 1/sqrt((2*pi)^dim * det(cov(ekr.p))) - oneval = mt.weights[i] * nscl * ker(mt.manifold, ekr, p, 0.5, distanceMalahanobisSq) + _ekr = getfield(ApproxManifoldProducts,HL.name.name)(mean(ekr.p), bw_scl*cov(ekr.p)) + nscl = 1/sqrt((2*pi)^dim * det(cov(_ekr.p))) + oneval = mt.weights[i] * nscl * ker(mt.manifold, _ekr, p, 0.5, distanceMalahanobisSq) # @info "EVAL" i oneval sumval += oneval end @@ -334,6 +349,9 @@ leaveOneOutLogL( ) = entropy(mt, bw_scl) +(mt::ManellicTree)( + evalpt::AbstractArray, +) = evaluate(mt, evalpt) # ## Pseudo code diff --git a/src/services/ManifoldKernelDensity.jl b/src/services/ManifoldKernelDensity.jl index f324145..bf14fbe 100644 --- a/src/services/ManifoldKernelDensity.jl +++ b/src/services/ManifoldKernelDensity.jl @@ -59,7 +59,8 @@ function ManifoldKernelDensity( partial::L=nothing, infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(M, u0)), dims::Int=manifold_dimension(M), - bw::Union{<:AbstractVector{<:Real},Nothing}=nothing + bw::Union{<:AbstractVector{<:Real},<:AbstractMatrix{<:Real},Nothing}=nothing, + belmodel::Function = (a,b,aF,dF) -> KernelDensityEstimate.kde!(a, collect(b), aF, dF) # collect(b) but error length(::Nothing) ) where {P,L} # # FIXME obsolete @@ -79,7 +80,8 @@ function ManifoldKernelDensity( _bw[mask] .= 1.0 end addopT, diffopT, _, _ = buildHybridManifoldCallbacks(manis) - bel = KernelDensityEstimate.kde!(arr, collect(_bw), addopT, diffopT) + bel = belmodel(arr,_bw,addopT,diffopT) + # bel = KernelDensityEstimate.kde!(arr, collect(_bw), addopT, diffopT) return ManifoldKernelDensity(M, bel, partial, u0, infoPerCoord) end @@ -226,6 +228,9 @@ end function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P} + _round(s::AbstractArray; kw...) = round.(s[:];kw...) + _round(s::AbstractVector{<:AbstractMatrix}; kw...) = round.(s[1][:];kw...) + printstyled(io, "ManifoldKernelDensity{", bold=true, color=:blue ) println(io) printstyled(io, " M", bold=true, color=:magenta ) @@ -248,7 +253,7 @@ function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P} println(io, " prtl: ", mkd._partial) bw = getBW(mkd.belief)[:,1] pvec = isPartial(mkd) ? mkd._partial : collect(1:length(bw)) - println(io, " bws: ", getBandwidth(mkd, true) .|> x->round(x,digits=4)) + println(io, " bws: ", getBandwidth(mkd, true) |> x->_round(x;digits=4)) # .|> x->round(x,digits=4)) println(io, " ipc: ", getInfoPerCoord(mkd, true) .|> x->round(x,digits=4)) print(io, " mean: ") try diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 38b66c4..0b77f40 100644 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -159,11 +159,51 @@ pts = [zeros(1) for _ in 1:100] bw = ones(1,1) mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw, kernel=AMP.MvNormalKernel) -AMP.evaluate(mtree, SA[0.0;]) +@test isapprox( 0.4, AMP.evaluate(mtree, SA[0.0;]); atol=0.1) AMP.evalAvgLogL(mtree, [randn(1) for _ in 1:5]) @show AMP.entropy(mtree) +# Vector bw required for backward compat with legacy belief structure +mtreeV = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[1.0;], kernel=AMP.MvNormalKernel) + + +bel = manikde!( + M, + pts; + bw, + belmodel = (a,b,aF,dF) -> ApproxManifoldProducts.buildTree_Manellic!( + M, + pts; + kernel_bw=b, + kernel=AMP.MvNormalKernel + ) +) + + +@test isapprox( 0.4, bel([0.0;]); atol=0.1) + +## +end + + +@testset "Manellic tree bandwidth evaluation / optimization" begin +## load know test data test + +json_string = read(joinpath(DATADIR,"manellic_test_data.json"), String) +dict = JSON3.read(json_string, Dict{Symbol,Vector{Float64}}) + +M = TranslationGroup(1) +pts = [[v;] for v in dict[:evaltest_1_pts]] +bw = reshape(dict[:evaltest_1_bw],1,1) +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) + +AMP.evalAvgLogL(mtree, pts) + +@test AMP.evalAvgLogL(mtree, pts, 1.1) < AMP.evalAvgLogL(mtree, pts, 1.0) < AMP.evalAvgLogL(mtree, pts, 0.9) + +# do linesearch for best selection of bw_scl + ## end \ No newline at end of file From b59df19e642435ae4c59ec986f71f69de6803e10 Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Thu, 14 Mar 2024 03:53:33 -0700 Subject: [PATCH 04/31] start Manellic bw optim via section search (#278) * start Manellic bw optim via section search Co-authored-by: Johannes Terblanche * manellic many bug fixes towards optim kbw selection * test manellic bug fix * fix testmanellic bug --------- Co-authored-by: Johannes Terblanche --- src/API.jl | 34 ++--- src/ApproxManifoldProducts.jl | 2 +- src/services/KernelEval.jl | 42 +++++- src/services/ManellicTree.jl | 225 ++++++++++++++++++++++++------ test/manellic/testManellicTree.jl | 147 ++++++++++++++++++- 5 files changed, 381 insertions(+), 69 deletions(-) diff --git a/src/API.jl b/src/API.jl index 7757de2..102d7b1 100644 --- a/src/API.jl +++ b/src/API.jl @@ -30,21 +30,24 @@ plot( x=getPoints(pq)[1,:], y=getPoints(pq)[2,:], Geom.histogram2d ) # TODO, convenient plotting (work in progress...) ``` """ -function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity}, - mani::M=ff[1].manifold; - makeCopy::Bool=false, - Niter::Integer=1, - # partialDimsWorkaround=1:MB.manifold_dimension(mani), - ndims::Integer=maximum([0;Ndim.(ff)]), - N::Integer = maximum([0;Npts.(ff)]), - u0 = getPoints(ff[1], false)[1], - oldPoints::AbstractVector{P}= [identity_element(mani, u0) for i in 1:N], - 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} +function manifoldProduct( + ff::AbstractVector{<:ManifoldKernelDensity}, + mani::M=ff[1].manifold; + makeCopy::Bool=false, + Niter::Integer=1, + # partialDimsWorkaround=1:MB.manifold_dimension(mani), + ndims::Integer=maximum([0;Ndim.(ff)]), + N::Integer = maximum([0;Npts.(ff)]), + u0 = getPoints(ff[1], false)[1], + oldPoints::AbstractVector{P}= [identity_element(mani, u0) for i in 1:N], + addEntropy::Bool=true, + recordLabels::Bool=false, + selectedLabels::Vector{Vector{Int}}=Vector{Vector{Int}}(), + _randU=Vector{Float64}(), + _randN=Vector{Float64}(), + logger=ConsoleLogger(), + bws = 0 == length(ff) ? [1.0;] : ones(ndims), +) where {M <: MB.AbstractManifold, P} # # check quick exit if 1 == length(ff) @@ -66,7 +69,6 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity}, glbs = KDE.makeEmptyGbGlb(); glbs.recordChoosen = recordLabels - bws = 0 == length(ff) ? [1.0;] : ones(ndims) # MAKE SURE inplace ends up as matrix of coordinates from incoming ::Vector{P} oldpts = _pointsToMatrixCoords(mani, oldPoints) # FIXME currently assumes oldPoints are in coordinates... diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index ef84444..d143d34 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -31,7 +31,7 @@ import Random: rand import Base: *, isapprox, convert, show, eltype import LinearAlgebra: rotate!, det -import Statistics: mean, std, cov, var +import Statistics: mean, std, cov, var, entropy import KernelDensityEstimate: getPoints, getBW, evalAvgLogL, entropy const MB = ManifoldsBase diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index 31ed009..3fe6ab8 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -18,8 +18,46 @@ function MvNormalKernel(m::AbstractVector,c::AbstractArray) MvNormalKernel(;p, sqrt_iΣ) end -Statistics.mean(m::MvNormalKernel) = m.p.μ -Statistics.cov(m::MvNormalKernel) = m.p.Σ # note also about m.sqrt_iΣ +Statistics.mean(m::MvNormalKernel) = mean(m.p) # m.p.μ +Statistics.cov(m::MvNormalKernel) = cov(m.p) # note also about m.sqrt_iΣ +Statistics.std(m::MvNormalKernel) = sqrt(cov(m)) + +""" + $SIGNATURES + +Transform `T=RS` from unit covariance `D` to instance covariance `Σ = TD`. + +Notes: +- Geometric interpretation of the covariance matrix, Fig. 10, https://users.cs.utah.edu/~tch/CS6640F2020/resources/A%20geometric%20interpretation%20of%20the%20covariance%20matrix.pdf + - Eigen decomp: `Σ^2 V = VL` => `Σ^2 = VL(V^-1) = RL(R^-1) = RSS(R^-1)` => `T=RS` +""" +function covTransformNormalized(Σ::AbstractMatrix) + F = eigen(Σ) + R = F.vectors + L = diagm(F.values) + S = sqrt(L) + return R*S +end + +function Base.show(io::IO, mvk::MvNormalKernel) + μ = mean(mvk) + Σ2 = cov(mvk) + # Σ=sqrt(Σ2) + d = length(μ) + print(io, "MvNormalKernel(d=",d) + print(io,",μ=",round.(μ;digits=3)) + print(io,",Σ^2=[",round(Σ2[1];digits=3)) + if 1 Date: Mon, 18 Mar 2024 04:31:56 -0700 Subject: [PATCH 05/31] manellic tree fix permutation and tests (#279) Co-authored-by: Johannes Terblanche --- src/entities/ManellicTree.jl | 1 + src/services/ManellicTree.jl | 91 +++++++++------ test/manellic/testManellicTree.jl | 184 ++++++++++++++++++++++-------- 3 files changed, 192 insertions(+), 84 deletions(-) diff --git a/src/entities/ManellicTree.jl b/src/entities/ManellicTree.jl index 5f1bd85..7a99dcb 100644 --- a/src/entities/ManellicTree.jl +++ b/src/entities/ManellicTree.jl @@ -25,6 +25,7 @@ struct ManellicTree{M,D<:AbstractVector,N,HL,HT} # kernels::ArrayPartition{<:Number,KT} leaf_kernels::SizedVector{N,HL} tree_kernels::SizedVector{N,HT} + segments::SizedVector{N,Set{Int}} left_idx::MVector{N,Int} right_idx::MVector{N,Int} end diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 1b563e1..69d5fc9 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -142,6 +142,7 @@ Give vector of manifold points and split along largest covariance (i.e. major di DeVNotes: - FIXME: upgrade to Manopt version - https://github.com/JuliaRobotics/ApproxManifoldProducts.jl/issues/277 +- FIXME use manifold mean and cov calculation instead """ function splitPointsEigen( M::AbstractManifold, @@ -152,10 +153,11 @@ function splitPointsEigen( # len = length(r_PP) + # important, covariance is calculated around mean of points, which enables log to avoid singularities # do calculations around mean point on manifold, i.e. support Riemannian p = mean(M, r_PP) - r_XXp = log.(Ref(M), Ref(p), r_PP) + r_XXp = log.(Ref(M), Ref(p), r_PP) # FIXME replace with on-manifold distance r_CCp = vee.(Ref(M), Ref(p), r_XXp) D = manifold_dimension(M) @@ -185,35 +187,36 @@ function splitPointsEigen( # this is a local test around base point p (not at global 0) mask = 0 .<= (ax_CCp .|> s->s[1]) - # TODO ALLOW BOTH BALANCED OR UNBALANCED MASK RETRIEVAL, STARTING WITH FORCED MASK BALANCING - # rebalance if stochastic nearest estimates fall in wrong mask - function _flipmask_minormax!( - smlmask, - bigmask, - data; - argminmax::Function = argmin - ) - N = length(smlmask) - # move minimum mask points over to imask - for k in 1:((sum(bigmask) - sum(smlmask)) ÷ 2) - # keep flipping the minimum element from mask into imask set - # note using first coord, ie.. x-axis as the split axis: `s->s[1]` - mlis = (1:sum(bigmask)) - ami = argminmax(view(data, bigmask)) - idx = mlis[ ami ] - # get idx from orginal list - flipidx = view(1:N, bigmask)[idx] - data[flipidx] - bigmask[flipidx] = xor(bigmask[flipidx], true) - smlmask[flipidx] = xor(smlmask[flipidx], true) + # TODO ALLOW BOTH BALANCED OR UNBALANCED MASK RETRIEVAL, STARTING WITH FORCED MASK BALANCING + # NOTE, rebalancing reason: deadcenter of covariance is not halfway between points (unconfirmed) + # rebalance if stochastic nearest estimates fall in wrong mask + function _flipmask_minormax!( + smlmask, + bigmask, + data; + argminmax::Function = argmin + ) + N = length(smlmask) + # move minimum mask points over to imask + for k in 1:((sum(bigmask) - sum(smlmask)) ÷ 2) + # keep flipping the minimum element from mask into imask set + # note using first coord, ie.. x-axis as the split axis: `s->s[1]` + mlis = (1:sum(bigmask)) + ami = argminmax(view(data, bigmask)) + idx = mlis[ ami ] + # get idx from orginal list + flipidx = view(1:N, bigmask)[idx] + data[flipidx] + bigmask[flipidx] = xor(bigmask[flipidx], true) + smlmask[flipidx] = xor(smlmask[flipidx], true) + end + nothing end - nothing - end - - imask = xor.(mask,true) - ax_CC1 = (s->s[1]).(ax_CCp) - _flipmask_minormax!(imask, mask, ax_CC1; argminmax=argmin) - _flipmask_minormax!(mask, imask, ax_CC1; argminmax=argmax) + + imask = xor.(mask,true) + ax_CC1 = (s->s[1]).(ax_CCp) + _flipmask_minormax!(imask, mask, ax_CC1; argminmax=argmin) + _flipmask_minormax!(mask, imask, ax_CC1; argminmax=argmax) # return rotated coordinates and split mask ax_CCp, mask, kernel(p, cv) @@ -226,9 +229,9 @@ _getright(i::Integer) = 2*i + 1 function buildTree_Manellic!( mtree::ManellicTree{MT,D,N}, - index::Integer, - low::Integer, # bottom index of segment - high::Integer; # top index of segment; + index::Integer, # tree node root=1,left=2n,right=left+1 + low::Integer, # bottom index of segment + high::Integer; # top index of segment; kernel = MvNormal, kernel_bw = nothing, leaf_size = 1 @@ -251,20 +254,21 @@ function buildTree_Manellic!( ido = view(mtree.permute, idc) # split the slice of order-permuted data ax_CCp, mask, knl = splitPointsEigen(M, view(mtree.data, ido); kernel, kernel_bw=_kernel_bw) + imask = xor.(mask, true) # sort the data as 'small' and 'big' elements either side of the eigen split big = view(ido, mask) - sml = view(ido, xor.(mask, true)) - # reorder the slice portion of the permutation with new ordering + sml = view(ido, imask) + # inplace reorder the slice portion of mtree.permute towards accending ido .= SA[sml...; big...] npts = high - low + 1 - mid_idx = low + sum(mask) - 1 + mid_idx = low + sum(imask) - 1 # @info "BUILD" index low sum(mask) mid_idx high _getleft(index) _getright(index) lft = mid_idx <= low ? low : _getleft(index) - rgt = high < mid_idx+1 ? high : _getright(index) + rgt = high <= mid_idx+1 ? high : _getright(index) if leaf_size < npts if lft != low # mid_idx @@ -281,6 +285,7 @@ function buildTree_Manellic!( # FIXME, replace with just the kernel choice, not hyper such and such needed? if index < N mtree.tree_kernels[index] = knl # HyperEllipse(knl.μ, knl.Σ.mat) + mtree.segments[index] = Set(ido) end return mtree @@ -348,12 +353,13 @@ function buildTree_Manellic!( lkern, # MVector{len,lknlT}(undef), SizedVector{len,tknlT}(undef), # SizedVector{len,tknlT}(undef), + SizedVector{len,Set{Int}}(undef), + MVector{len,Int}(undef), MVector{len,Int}(undef), - MVector{len,Int}(undef) ) # - return buildTree_Manellic!( + tosort_leaves = buildTree_Manellic!( mtree, 1, # start at root 1, # spanning all data @@ -361,6 +367,15 @@ function buildTree_Manellic!( kernel, kernel_bw ) + + # manual reset leaves in the order discovered + permute!(tosort_leaves.leaf_kernels, tosort_leaves.permute) + # dupl = deepcopy(tosort_leaves.leaf_kernels) + # for (k,i) in enumerate(tosort_leaves.permute) + # tosort_leaves[i] = dupl.leaf_kernels[k] + # end + + return tosort_leaves end """ diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index fcd2777..327b4a6 100644 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -2,6 +2,7 @@ # using Revise using Test using ApproxManifoldProducts +using Random using StaticArrays using TensorCast using Manifolds @@ -112,8 +113,85 @@ end @testset "ManellicTree construction 1D" begin ## +function testMDEConstr( + pts::AbstractVector{<:AbstractVector{<:Real}}, + permref = sortperm(pts, by=s->getindex(s,1)); + lseg = 1:2, + rseg = 3:4 +) + # check permutation + M = TranslationGroup(1) + bw = [1.0] + + mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) + @test permref == mtree.permute + @test isapprox( mean(M, pts), mean(mtree.tree_kernels[1]); atol=1e-10) + @test Set(mtree.segments[1]) == Set(union(lseg, rseg)) + @test Set(mtree.segments[2]) == Set(mtree.permute[lseg]) + @test Set(mtree.segments[3]) == Set(mtree.permute[rseg]) + @test isapprox( mean(M, pts[mtree.permute[lseg]]), mean(mtree.tree_kernels[2]); atol=1e-10) + @test isapprox( mean(M, pts[mtree.permute[rseg]]), mean(mtree.tree_kernels[3]); atol=1e-10) + nothing +end + +## for 4 values + +# manual orders +testMDEConstr( [[0.;],[1.],[3.;],[6.;]] ) +testMDEConstr( [[0.;],[1.],[6.;],[3.;]] ) +testMDEConstr( [[0.;],[3.],[1.;],[6.;]] ) +testMDEConstr( [[1.;],[0.],[3.;],[6.;]] ) +testMDEConstr( [[1.;],[0.],[6.;],[3.;]] ) +testMDEConstr( [[1.;],[6.],[0.;],[3.;]] ) +testMDEConstr( [[6.;],[1.],[3.;],[0.;]] ) + +testMDEConstr( [[0.9497270480266986;], [-0.5973125859935883;], [-0.6031001429225558;], [-0.3971695179687664;]] ) + + +# randomized orders for 4 values +pts = [[0.;],[1.],[3.;],[6.;]] +for i in 1:10 + testMDEConstr( pts[shuffle(1:4)] ) +end + + +## for 5 values + +testMDEConstr( [[0.;],[1.],[3.;],[6.;],[10.;]]; lseg=1:3, rseg=4:5) +testMDEConstr( [[0.;],[1.],[6.;],[3.;],[10.;]]; lseg=1:3, rseg=4:5) + +# randomized orders for 5 values +pts = [[0.;],[1.],[3.;],[6.;],[10.;],] +for i in 1:10 + testMDEConstr( pts[shuffle(1:length(pts))], lseg=1:3, rseg=4:5) +end + +## for 7 values + +# randomized orders for 7 values +pts = [[0.;],[1.],[3.;],[6.;],[10.;],[15.;],[21.;]] +for i in 1:10 + testMDEConstr( pts[shuffle(1:length(pts))]; lseg=1:4,rseg=5:7 ) +end + + +# M = TranslationGroup(1) -pts = [randn(1) for _ in 1:100] +pts = [randn(1) for _ in 1:8] +for i in 1:10 + _pts = pts[shuffle(1:length(pts))] + testMDEConstr( _pts; lseg=1:4,rseg=5:8 ) +end + +## +end + + +@testset "ManellicTree 1D basic construction and evaluations" begin +## + +M = TranslationGroup(1) +pts = [randn(1) for _ in 1:128] mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel=AMP.MvNormalKernel) AMP.evaluate(mtree, SA[0.0;]) @@ -128,13 +206,6 @@ pts = [[v;] for v in dict[:evaltest_1_pts]] bw = reshape(dict[:evaltest_1_bw],1,1) mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) -# for (i,v) in enumerate(dict[:evaltest_1_at]) -# # @show AMP.evaluate(mtree, [v;]), dict[:evaltest_1_dens][i] -# @test isapprox(dict[:evaltest_1_dens][i], AMP.evaluate(mtree, [v;])) -# end -# isapprox(dict[:evaltest_1_dens][5], AMP.evaluate(mtree, [dict[:evaltest_1_at][5]])) -# eval test ref Normal(0,1) - np = Normal(0,1) h = 0.1 xx = -5:h:5 @@ -149,15 +220,24 @@ end # lines(xx, yy_, color=:red) # ref # lines!(xx, yy, color=:blue) # test +# test sorting order of data +permref = sortperm(pts, by=s->getindex(s,1)) -# check permutation -M = TranslationGroup(1) -pts = [[0.;],[1.],[2.;],[3.;]] -bw = [1.0] -mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) +@test 0 == sum(permref - mtree.permute) + +@test 0 == sum(collect(sortperm(mtree.leaf_kernels; by=s->mean(s))) - collect(1:length(mtree.leaf_kernels))) + +#and leaf kernel sorting +@test norm((pts[mtree.permute] .- mean.(mtree.leaf_kernels)) .|> s->s[1]) < 1e-6 -# TODO untested -@test mtree.permute == [1;2;3;4] + + +# for (i,v) in enumerate(dict[:evaltest_1_at]) +# # @show AMP.evaluate(mtree, [v;]), dict[:evaltest_1_dens][i] +# @test isapprox(dict[:evaltest_1_dens][i], AMP.evaluate(mtree, [v;])) +# end +# isapprox(dict[:evaltest_1_dens][5], AMP.evaluate(mtree, [dict[:evaltest_1_at][5]])) +# eval test ref Normal(0,1) ## end @@ -200,8 +280,6 @@ bel = manikde!( end -# - @testset "Manellic tree bandwidth evaluation" begin ## load know test data test @@ -245,6 +323,8 @@ end @test cost(1e-3) < cost(1e-2) < cost(1e-1) < cost(1e-0) @test cost(1e2) < cost(1e1) < cost(1e0) +# S = [1e-3; 1e-2; 1e-1; 1e0; 1e1; 1e2] +# Y = cost.(S) ## end @@ -254,8 +334,8 @@ end ## M = TranslationGroup(1) -# pts = [[0.;],[1.],[2.;],[3.;]] -pts = [randn(1) for _ in 1:128] +# pts = [[0.;],[0.1],[0.2;],[0.3;]] +pts = [1*randn(1) for _ in 1:64] bw = [1.0] mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) @@ -285,62 +365,74 @@ AMP.entropy(mtree_0, upper[1]) # iterations # rel_tol: The relative tolerance used for determining convergence. Defaults to sqrt(eps(T)) # abs_tol: The absolute tolerance used for determining convergence. Defaults to eps(T) -cost(s) = begin - mtr = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[s;;],kernel=AMP.MvNormalKernel) +cost(_pts, σ) = begin + mtr = ApproxManifoldProducts.buildTree_Manellic!(M, _pts; kernel_bw=[σ;;],kernel=AMP.MvNormalKernel) AMP.entropy(mtr) end + +S = 0.005:0.05:3 +Y = S .|> s->cost(pts,s^2) + +# should pass the optimal kbw somewhere in the given range +@test any(0 .< diff(Y)) + +# and optimize + res = Optim.optimize( - cost, - 0.05, 0.8, Optim.GoldenSection() + (s)->cost(pts,s^2), + 0.05, 3.0, Optim.GoldenSection() ) best_cov = Optim.minimizer(res) -@test_broken isapprox(0.38, best_cov; atol=0.15) +@test isapprox(0.38, best_cov; atol=0.2) -# test why broken - -function cost(s) - mtr = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[s;;],kernel=AMP.MvNormalKernel) - # AMP.entropy(mtr) - AMP.expectedLogL(mtr, getPoints(mtr), 1, true) +## end -XX = 0.05:0.05:0.3 -YY = cost.(XX) -# should pass the optimal kbw somewhere in the given range -@test_broken any(0 .< diff(YY)) +# TODO +# @testset "Manellic tree bandwidth optimize n-dim RLM" begin +# ## -## -end +# ## +# end ## -# using GLMakie +# # using GLMakie + +# M = TranslationGroup(1) + +# __pts = [1*randn(1) for _ in 1:64] -# f = Figure() +# ## + +# f = Figure() # ax = f[1, 1] = Axis(f; xscale=log10,yscale=log10) -# lines!(S, -Y, color=:blue, label="Manellic") +# ## -# f[1, 2] = Legend(f, ax, "Entropy R&D", framevisible = false) +# SFT = 0.0 +# pts = [p .+ SFT for p in __pts] +# ## + +# S = 0.005:0.01:2 +# Y = S .|> s->cost(pts,s^2) + +# lines!(S, Y .+ 0.1, color=:blue, label="Manellic $SFT") + +# f[1, 2] = Legend(f, ax, "Entropy R&D", framevisible = false) # f ## -# TODO -# @testset "Manellic tree bandwidth optimize n-dim RLM" begin -# ## - -# ## -# end # \ No newline at end of file From 2a1b07d67806b48979b1a993a08c9aea5abb10cd Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Mon, 25 Mar 2024 09:51:03 -0400 Subject: [PATCH 06/31] Sequential Gibbs on manellic (#280) * Update ManellicTree.jl * test remote commit via new testset block manellic * calc and test product Gaussians * hotix AbstractManifold for calcProdGaus * towards std kernel eval and gibbs * sampleProductGibbsSeq functional but bugs * bug fix on calcProductKernelLabels * update first test on manellic seq Gibbs product approx * typo fix test --- src/ApproxManifoldProducts.jl | 2 +- src/CommonUtils.jl | 69 +++++++++---- src/entities/KernelEval.jl | 6 +- src/services/KernelEval.jl | 25 +++-- src/services/ManellicTree.jl | 160 +++++++++++++++++++++++++++--- test/manellic/testManellicTree.jl | 75 +++++++++++++- 6 files changed, 292 insertions(+), 45 deletions(-) diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index d143d34..b1db91b 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -32,7 +32,7 @@ import Random: rand import Base: *, isapprox, convert, show, eltype import LinearAlgebra: rotate!, det import Statistics: mean, std, cov, var, entropy -import KernelDensityEstimate: getPoints, getBW, evalAvgLogL, entropy +import KernelDensityEstimate: getPoints, getBW, evalAvgLogL, entropy, evaluate const MB = ManifoldsBase const CTs = CoordinateTransformations diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index 44e8d01..715a965 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -117,12 +117,13 @@ Notes - https://ccrma.stanford.edu/~jos/sasp/Product_Two_Gaussian_PDFs.html - Pennec, X. Intrinsic Statistics on Riemannian Manifolds: Basic Tools for Geometric Measurements, HAL Archive, 2011, Inria, France. """ -function calcProductGaussians(M::AbstractManifold, - μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P - Σ_::Union{Nothing,<:AbstractVector{S},<:NTuple{N,S}}; - dim::Integer=manifold_dimension(M), - Λ_ = inv.(Σ_), - ) where {N,P,S<:AbstractMatrix{<:Real}} +function calcProductGaussians( + M::AbstractManifold, + μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P + Σ_::Union{Nothing,<:AbstractVector{S},<:NTuple{N,S}}; + dim::Integer=manifold_dimension(M), + Λ_ = inv.(Σ_), +) where {N,P,S<:AbstractMatrix{<:Real}} # # calc sum of covariances Λ = zeros(MMatrix{dim,dim}) @@ -148,20 +149,52 @@ end # additional support case where covariances are passed as diagonal-only vectors # still pass nothing, to avoid stack overflow. Only Λ_ is needed further -calcProductGaussians( M::AbstractManifold, - μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, - Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; - dim::Integer=manifold_dimension(M), - Λ_ = map(s->diagm( 1.0 ./ s), Σ_), - ) where {N,P,S<:AbstractVector} = calcProductGaussians(M, μ_, nothing; dim=dim, Λ_=Λ_ ) +calcProductGaussians( + M::AbstractManifold, + μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, + Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; + dim::Integer=manifold_dimension(M), + Λ_ = map(s->diagm( 1.0 ./ s), Σ_), +) where {N,P,S<:AbstractVector} = calcProductGaussians(M, μ_, nothing; dim, Λ_=Λ_ ) # -calcProductGaussians( M::AbstractManifold, - μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}; - dim::Integer=manifold_dimension(M), - Λ_ = diagm.( (1.0 ./ μ_) ), - ) where {N,P} = calcProductGaussians(M, μ_, nothing; dim=dim, Λ_=Λ_ ) -# +calcProductGaussians( + M::AbstractManifold, + μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}; + dim::Integer=manifold_dimension(M), + Λ_ = diagm.( (1.0 ./ μ_) ), +) where {N,P} = calcProductGaussians(M, μ_, nothing; dim, Λ_=Λ_ ) + + + +function calcProductGaussians( + M::AbstractManifold, + comps::AbstractVector{<:MvNormalKernel}, +) + # + μ_ = mean.(comps) + Σ_ = cov.(comps) + + _μ, _Σ = calcProductGaussians(M, μ_, Σ_) + + return MvNormalKernel(_μ, _Σ) + # d = manifold_dimension(M) + # # TODO avoid recomputing covariance matrix inverses all the time + # _Sigma2 = zeros(d,d) + # _Sig2mu = zeros(d) + # for c in comps + # concentration = inv(cov(c)) + # _Sigma2 += concentration + # _Sig2mu += concentration * mean(c) + # end + + # Sigma2 = inv(_Sigma2) + # mu = Sigma2 * _Sig2mu + + # return MvNormalKernel(mu, Sigma2) +end + + function _update!(dst::MN, src::MN) where {MN <: ManifoldKernelDensity} diff --git a/src/entities/KernelEval.jl b/src/entities/KernelEval.jl index 6af1c02..5edf504 100644 --- a/src/entities/KernelEval.jl +++ b/src/entities/KernelEval.jl @@ -2,8 +2,12 @@ abstract type AbstractKernel end -@kwdef struct MvNormalKernel{T,M,iM} <: AbstractKernel +@kwdef struct MvNormalKernel{P,T,M,iM} <: AbstractKernel + """ On-manifold point representing center (mean) of the MvNormal distrubtion """ + μ::P + """ Zero-mean normal distrubtion with covariance """ p::MvNormal{T,M} # TDB might already be covered in p.Σ.chol but having issues with SymPD (not particular to this AMP repo) + """ Manually maintained square root concentration matrix for faster compute, TODO likely duplicate of existing Distrubtions.jl functionality. """ sqrt_iΣ::iM = sqrt(inv(p.Σ)) end diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index 3fe6ab8..fd4f2d2 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -7,21 +7,34 @@ function projectSymPosDef(c::AbstractMatrix) issymmetric(_c) ? _c : project(SymmetricPositiveDefinite(s[1]),_c,_c) end - +# FIXME, REMOVE TYPE DISPLACEMENT Base.eltype(mt::MvNormalKernel) = eltype(mt.p) -function MvNormalKernel(m::AbstractVector,c::AbstractArray) - _c = projectSymPosDef(c) - p=MvNormal(m,_c) +function MvNormalKernel(μ::AbstractVector,Σ::AbstractArray) + _c = projectSymPosDef(Σ) + p=MvNormal(_c) # NOTE, TBD, why not sqrt(inv(p.Σ)), this had an issue seemingly internal to PDMat.chol which breaks an already forced SymPD matrix to again be not SymPD??? sqrt_iΣ = sqrt(inv(_c)) - MvNormalKernel(;p, sqrt_iΣ) + MvNormalKernel(;μ, p, sqrt_iΣ) end -Statistics.mean(m::MvNormalKernel) = mean(m.p) # m.p.μ +Statistics.mean(m::MvNormalKernel) = m.μ # mean(m.p) # m.p.μ Statistics.cov(m::MvNormalKernel) = cov(m.p) # note also about m.sqrt_iΣ Statistics.std(m::MvNormalKernel) = sqrt(cov(m)) + +function evaluate( + M::AbstractManifold, + ekr::MvNormalKernel, + p +) + # + dim = manifold_dimension(M) + nscl = 1/sqrt((2*pi)^dim * det(cov(ekr))) + return nscl * ker(M, ekr, p, 0.5, distanceMalahanobisSq) +end + + """ $SIGNATURES diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 69d5fc9..03fd3e9 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -18,6 +18,19 @@ getWeights( mt::ManellicTree ) = view(mt.weights, mt.permute) + +""" + $SIGNATURES + +Return leaf kernel associated with input data element `i` (i.e. `permuted=true`). +Else when set to `permuted=false` return the sorted leaf_kernel `i` (different from unsorted input data number). +""" +getKernel( + mt::ManellicTree, + i::Int, + permuted::Bool = true +) = mt.leaf_kernels[mt.permute[i]] + uniWT(mt::ManellicTree) = 1===length(union(diff(getWeights(mt)))) function uniBW( @@ -110,6 +123,26 @@ end Base.show(io::IO, ::MIME"text/plain", mt::ManellicTree) = show(io, mt) +# case for identical types not requiring any conversions +Base.convert( + ::Type{MvNormalKernel{P,T,M,iM}}, + src::MvNormalKernel{P,T,M,iM}, +) where {P,T,M,iM} = src + +function Base.convert( + ::Type{MvNormalKernel{P,T,M,iM}}, + src::MvNormalKernel, +) where {P,T,M,iM} + # + _matType(::Type{Distributions.PDMats.PDMat{_F,_M}}) where {_F,_M} = _M + μ = P(src.μ) + p = MvNormal(_matType(M)(cov(src.p))) + sqrt_iΣ = iM(src.sqrt_iΣ) + MvNormalKernel{P,T,M,iM}(μ, p, sqrt_iΣ) +end + + + # covariance function eigenCoords( f_CVp::AbstractMatrix @@ -284,7 +317,8 @@ function buildTree_Manellic!( # set HyperEllipse at this level in tree # FIXME, replace with just the kernel choice, not hyper such and such needed? if index < N - mtree.tree_kernels[index] = knl # HyperEllipse(knl.μ, knl.Σ.mat) + _knl = convert(eltype(mtree.tree_kernels), knl) + mtree.tree_kernels[index] = _knl # HyperEllipse(knl.μ, knl.Σ.mat) mtree.segments[index] = Set(ido) end @@ -394,7 +428,7 @@ DevNotes: function evaluate( mt::ManellicTree{M,D,N,HL}, p, - bw_scl::Real = 1, + # bw_scl::Real = 1, LOO::Bool = false, ) where {M,D,N,HL} # force function barrier, just to be sure dyndispatch is limited @@ -404,17 +438,15 @@ function evaluate( pts = getPoints(mt) w = getWeights(mt) - dim = manifold_dimension(mt.manifold) sumval = 0.0 # FIXME, brute force for loop for (i,t) in enumerate(pts) - if !LOO || !isapprox(p, t) + if !LOO || !isapprox(p, t) # FIXME change isapprox to on-manifold version + # FIXME, is this assuming length(pts) and length(mt.leaf_kernels) are the same? ekr = mt.leaf_kernels[i] - _ekr = _F_(mean(ekr), bw_scl*cov(ekr)) - nscl = 1/sqrt((2*pi)^dim * det(cov(_ekr))) - nscl *= !LOO ? 1 : 1/(1-w[i]) - oneval = mt.weights[i] * nscl * ker(mt.manifold, _ekr, p, 0.5, distanceMalahanobisSq) - # @info "EVAL" i oneval + # TODO remember special handling for partials in the future + oneval = mt.weights[i] * evaluate(mt.manifold, ekr, p) + oneval *= !LOO ? 1 : 1/(1-w[i]) sumval += oneval end end @@ -426,7 +458,7 @@ end function expectedLogL( mt::ManellicTree{M,D,N}, epts::AbstractVector, - bw_scl::Real = 1, + # bw_scl::Real = 1, LOO::Bool = false ) where {M,D,N} T = Float64 @@ -434,7 +466,7 @@ function expectedLogL( eL = MVector{length(epts),T}(undef) for (i,p) in enumerate(epts) # LOO skip for leave-one-out - eL[i] = evaluate(mt, p, bw_scl, LOO) + eL[i] = evaluate(mt, p, LOO) end # set numerical tolerance floor zrs = findall(isapprox.(0,eL)) @@ -455,8 +487,7 @@ end entropy( mt::ManellicTree, - bw_scl::Real = 1, -) = -expectedLogL(mt, getPoints(mt), bw_scl, true) +) = -expectedLogL(mt, getPoints(mt), true) # leaveOneOutLogL( # mt::ManellicTree, @@ -517,4 +548,105 @@ end # _X1_ = X1_a*X1_b -# \ No newline at end of file +## WIP Sequential Gibbs Product development + + + +function sampleProductSeqGibbsLabel( + M::AbstractManifold, + proposals::AbstractVector, + treeLevel::Int = 1, # reserved for future use + MC = 3, +) + # + # how many incoming proposals + d = length(proposals) + + # # how many points per proposal + Ns = proposals .|> getPoints .|> length + + # TODO upgrade to multiscale + # start with random selection of labels + best_labels = [rand(1:n) for n in Ns] + + # pick the next leave-out proposal + for _ in MC, O in 1:d + # select a label from the not-selected proposal densities + sublabels = Tuple{Int,Int}[] + for s in setdiff(1:d, O) + slbl = best_labels[s] + push!(sublabels, (s,slbl)) + end + # prop = proposals[s] + + # TODO upgrade to map and tuples + components = map(sl->getKernel(proposals[sl[1]], sl[2], true), sublabels) + + # calc product of Gaussians from currently selected LOO-proposals + newO = calcProductGaussians(M, [components...]) + + # evaluate new sampling weights of points in out component + evat = getPoints(proposals[O]) # FIXME how should partials be handled here? + smw = zeros(length(evat)) + # FIXME, use multipoint evaluation such as NN (not just one point at a time) + for (i,ev) in enumerate(evat) + smw[i] = evaluate(M, newO, ev) + # δc = distanceMalahanobisCoordinates(M,newO,ev) + end + + smw ./= sum(smw) + + # smw = evaluate(newO, ) + p = Categorical(smw) + + # update label-distribution of out-proposal from product of selected LOO-proposal components + best_labels[O] = rand(p) + end + + # + + return best_labels +end + + +function sampleProductSeqGibbsLabels( + M::AbstractManifold, + proposals::AbstractVector, + treeLevel::Int = 1, # reserved for future use + MC = 3, + N::Int = round(Int, mean(length.(getPoints.(proposals)))) +) + # + d = length(proposals) + posterior_labels = Vector{NTuple{d,Int}}(undef,N) + + for i in 1:N + posterior_labels[i] = tuple(sampleProductSeqGibbsLabel(M,proposals,treeLevel,MC)...) + end + + posterior_labels +end + + +function calcProductKernelLabels( + M::AbstractManifold, + proposals::AbstractVector, + lbls::AbstractVector{<:NTuple} +) + # + + post = [] + + for lbs in lbls + props = MvNormalKernel[] + for (i,lb) in enumerate(lbs) + # selection of labels was done against sorted list of particles, hence false + push!(props, getKernel(proposals[i],lb,false)) + end + push!(post,calcProductGaussians(M,props)) + end + + return post +end + +## diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 327b4a6..d9e80a1 100644 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -293,7 +293,7 @@ mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=A AMP.expectedLogL(mtree, pts) -@test AMP.expectedLogL(mtree, pts, 1.1) < AMP.expectedLogL(mtree, pts, 1.0) < AMP.expectedLogL(mtree, pts, 0.9) +@test AMP.expectedLogL(mtree, pts) < Inf ## @@ -315,7 +315,7 @@ Y = [ApproxManifoldProducts.evaluate(_m_, [x;]) for x in XX] function cost(s) mtr = ApproxManifoldProducts.buildTree_Manellic!(M, pt; kernel_bw=[s;;],kernel=AMP.MvNormalKernel) # AMP.entropy(mtr) - AMP.expectedLogL(mtr, getPoints(mtr), 1, true) + AMP.expectedLogL(mtr, getPoints(mtr), true) end # optimal is somewhere in the single digits and basic monoticity outward @@ -356,8 +356,8 @@ bw_cov = (ucov + lcov)/2 mtree_0 = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw_cov,kernel=AMP.MvNormalKernel) lower = lcov / bw_cov upper = ucov / bw_cov -AMP.entropy(mtree_0, lower[1]) -AMP.entropy(mtree_0, upper[1]) +AMP.entropy(mtree_0) + # https://julianlsolvers.github.io/Optim.jl/stable/#user/minimization/#minimizing-a-univariate-function-on-a-bounded-interval @@ -385,7 +385,7 @@ res = Optim.optimize( ) best_cov = Optim.minimizer(res) -@test isapprox(0.38, best_cov; atol=0.2) +@test isapprox(0.5, best_cov; atol=0.3) ## @@ -434,5 +434,70 @@ end ## +@testset "Test utility functions for Gaussian products" begin +## + +M = TranslationGroup(1) + +g1 = ApproxManifoldProducts.MvNormalKernel([-1.0;],[4.0;;]) +g2 = ApproxManifoldProducts.MvNormalKernel([1.0;],[4.0;;]) + +g = ApproxManifoldProducts.calcProductGaussians(M, [g1; g2]) +@test isapprox( [0.0;], mean(g); atol=1e-6) +@test isapprox( [2.0;;], cov(g); atol=1e-6) + +g1 = ApproxManifoldProducts.MvNormalKernel([-1.0;],[4.0;;]) +g2 = ApproxManifoldProducts.MvNormalKernel([1.0;],[9.0;;]) + +g = ApproxManifoldProducts.calcProductGaussians(M, [g1; g2]) +@test isapprox( [-5/13;], mean(g); atol=1e-6) +@test isapprox( [36/13;;], cov(g); atol=1e-6) + +## +end + + +@testset "Product of two Manellic beliefs, Sequential Gibbs" begin +## + +M = TranslationGroup(1) + +pts = [randn(1).-1 for _ in 1:128] +p1 = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[0.1;;], kernel=ApproxManifoldProducts.MvNormalKernel) + +pts = [randn(1).+1 for _ in 1:128] +p2 = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[0.1;;], kernel=ApproxManifoldProducts.MvNormalKernel) + + +lbls = ApproxManifoldProducts.sampleProductSeqGibbsLabels(M, [p1; p2]) + +post = ApproxManifoldProducts.calcProductKernelLabels(M, [p1;p2], lbls) + +pts = mean.(post) +kernel_bw = mean(cov.(post)) + +mtr = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw, kernel=ApproxManifoldProducts.MvNormalKernel) + +@test isapprox( 0, mean(mtr.tree_kernels[1])[1]; atol=0.75) + +## +end + + +## + +# using GLMakie + + +# XX = [[s;] for s in -4:0.1:4] +# YY = ApproxManifoldProducts.evaluate.(Ref(mtr), XX) + +# lines((s->s[1]).(XX),YY, color=:magenta) + +# YY = ApproxManifoldProducts.evaluate.(Ref(p1), XX) +# lines!((s->s[1]).(XX),YY, color=:blue) +# YY = ApproxManifoldProducts.evaluate.(Ref(p2), XX) +# lines!((s->s[1]).(XX),YY, color=:red) + # \ No newline at end of file From 696d29cced951c43278abe29df59d422f2141c5d Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Sat, 20 Apr 2024 04:46:23 -0400 Subject: [PATCH 07/31] add kernel weight for manellic products (#281) * add kernel weight for manellic products * fix manellic tests, towards multiscale * fix tests on manellic * rm unused code --- src/entities/KernelEval.jl | 2 + src/entities/ManellicTree.jl | 3 + src/services/KernelEval.jl | 8 ++- src/services/ManellicTree.jl | 109 +++++++++++++++++++++++++----- test/manellic/testManellicTree.jl | 53 ++++++++++++++- 5 files changed, 154 insertions(+), 21 deletions(-) diff --git a/src/entities/KernelEval.jl b/src/entities/KernelEval.jl index 5edf504..d9587c9 100644 --- a/src/entities/KernelEval.jl +++ b/src/entities/KernelEval.jl @@ -10,4 +10,6 @@ abstract type AbstractKernel end # TDB might already be covered in p.Σ.chol but having issues with SymPD (not particular to this AMP repo) """ Manually maintained square root concentration matrix for faster compute, TODO likely duplicate of existing Distrubtions.jl functionality. """ sqrt_iΣ::iM = sqrt(inv(p.Σ)) + """ Nonparametric weight value """ + weight::Float64 = 1.0 end diff --git a/src/entities/ManellicTree.jl b/src/entities/ManellicTree.jl index 7a99dcb..5398ebe 100644 --- a/src/entities/ManellicTree.jl +++ b/src/entities/ManellicTree.jl @@ -28,5 +28,8 @@ struct ManellicTree{M,D<:AbstractVector,N,HL,HT} segments::SizedVector{N,Set{Int}} left_idx::MVector{N,Int} right_idx::MVector{N,Int} + + # workaround to overcome bug for StaticArrays `isdefined() != false` issue + _workaround_isdef_treekernel::Set{Int} end diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index fd4f2d2..76ed321 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -10,12 +10,16 @@ end # FIXME, REMOVE TYPE DISPLACEMENT Base.eltype(mt::MvNormalKernel) = eltype(mt.p) -function MvNormalKernel(μ::AbstractVector,Σ::AbstractArray) +function MvNormalKernel( + μ::AbstractVector, + Σ::AbstractArray, + weight::Real=1.0 +) _c = projectSymPosDef(Σ) p=MvNormal(_c) # NOTE, TBD, why not sqrt(inv(p.Σ)), this had an issue seemingly internal to PDMat.chol which breaks an already forced SymPD matrix to again be not SymPD??? sqrt_iΣ = sqrt(inv(_c)) - MvNormalKernel(;μ, p, sqrt_iΣ) + MvNormalKernel(;μ, p, sqrt_iΣ, weight=float(weight)) end Statistics.mean(m::MvNormalKernel) = m.μ # mean(m.p) # m.p.μ diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 03fd3e9..2c74e39 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -19,17 +19,28 @@ getWeights( ) = view(mt.weights, mt.permute) +# either leaf or tree kernel, if larger than N +leftIndex( + ::ManellicTree, + krnIdx::Int=1 +) = 2*krnIdx +rightIndex( + ::ManellicTree, + krnIdx::Int +) = 2*krnIdx+1 + + """ $SIGNATURES Return leaf kernel associated with input data element `i` (i.e. `permuted=true`). Else when set to `permuted=false` return the sorted leaf_kernel `i` (different from unsorted input data number). """ -getKernel( +getKernelLeaf( mt::ManellicTree, i::Int, permuted::Bool = true -) = mt.leaf_kernels[mt.permute[i]] +) = mt.leaf_kernels[permuted ? mt.permute[i] : i] uniWT(mt::ManellicTree) = 1===length(union(diff(getWeights(mt)))) @@ -138,7 +149,7 @@ function Base.convert( μ = P(src.μ) p = MvNormal(_matType(M)(cov(src.p))) sqrt_iΣ = iM(src.sqrt_iΣ) - MvNormalKernel{P,T,M,iM}(μ, p, sqrt_iΣ) + MvNormalKernel{P,T,M,iM}(μ, p, sqrt_iΣ, src.weight) end @@ -179,8 +190,9 @@ DeVNotes: """ function splitPointsEigen( M::AbstractManifold, - r_PP::AbstractVector{P}; - kernel = MvNormal, + r_PP::AbstractVector{P}, + weights::AbstractVector{<:Real} = ones(length(r_PP)); # FIXME, make static vector unless large + kernel = MvNormalKernel, kernel_bw = nothing, ) where {P <: AbstractArray} # @@ -251,8 +263,10 @@ function splitPointsEigen( _flipmask_minormax!(imask, mask, ax_CC1; argminmax=argmin) _flipmask_minormax!(mask, imask, ax_CC1; argminmax=argmax) + weight = sum(weights) + # return rotated coordinates and split mask - ax_CCp, mask, kernel(p, cv) + ax_CCp, mask, kernel(p, cv, weight) end @@ -286,7 +300,7 @@ function buildTree_Manellic!( # according to current index permutation (i.e. sort data as you build the tree) ido = view(mtree.permute, idc) # split the slice of order-permuted data - ax_CCp, mask, knl = splitPointsEigen(M, view(mtree.data, ido); kernel, kernel_bw=_kernel_bw) + ax_CCp, mask, knl = splitPointsEigen(M, view(mtree.data, ido), view(mtree.weights, ido); kernel, kernel_bw=_kernel_bw) imask = xor.(mask, true) # sort the data as 'small' and 'big' elements either side of the eigen split @@ -319,6 +333,7 @@ function buildTree_Manellic!( if index < N _knl = convert(eltype(mtree.tree_kernels), knl) mtree.tree_kernels[index] = _knl # HyperEllipse(knl.μ, knl.Σ.mat) + push!(mtree._workaround_isdef_treekernel, index) mtree.segments[index] = Set(ido) end @@ -390,6 +405,7 @@ function buildTree_Manellic!( SizedVector{len,Set{Int}}(undef), MVector{len,Int}(undef), MVector{len,Int}(undef), + Set{Int}() ) # @@ -551,41 +567,94 @@ end ## WIP Sequential Gibbs Product development +getKernelLeafAsTreeKer(mtr::ManellicTree{M,D,N,HL,HT}, idx::Int, permuted::Bool=true) where {M,D,N,HL,HT} = convert(HT,getKernelLeaf(mtr, idx % N, permuted)) + +function getKernelTree( + mtr::ManellicTree{M,D,N}, + currIdx::Int, +) where {M,D,N} + # must return sorted given name signature "Tree" + permuted = true + + if currIdx < N + return mtr.tree_kernels[currIdx] + else + return getKernelLeafAsTreeKer(mtr, currIdx, permuted) + end +end + + +# function getKernelsTreeLevelIdxs( +# mtr::ManellicTree{M,D,N}, +# level::Int, +# currIdx::Int = 1 +# ) where {M,D,N} + +# # go to children if idx level too high +# _idxlevel(idx) = floor(Int,log2(idx)) + 1 +# _exists(_i) = _i < N ? (_i in mtr._workaround_isdef_treekernel) : true # N from numPoints anyway # isdefined(mtr.leaf_kernels,_i) + +# #traverse tree +# if level == _idxlevel(currIdx) && _exists(currIdx) +# return Int[currIdx;] +# end + +# @warn "getKernelsTreeLevelIdxs still has corner case issues when tree_kernel[N]" maxlog=10 + +# # explore left and right +# return vcat( +# getKernelsTreeLevelIdxs(mtr,level,_getleft(currIdx)), +# getKernelsTreeLevelIdxs(mtr,level,_getright(currIdx)) +# ) +# end + +# function getKernelsTreeLevel( +# mtr::ManellicTree{M,D,N}, +# level::Int +# ) where {M,D,N} +# # kernels of that level +# levelIdxs = getKernelsTreeLevelIdxs(mtr,level) +# getKernelTree.(Ref(mtr),levelIdxs) +# end + + function sampleProductSeqGibbsLabel( M::AbstractManifold, proposals::AbstractVector, - treeLevel::Int = 1, # reserved for future use MC = 3, ) # # how many incoming proposals d = length(proposals) - # # how many points per proposal + ## TODO sample at multiscale levels on the tree, starting at treeLevel=1 + + # how many points per proposal at this depth level of the belief tree Ns = proposals .|> getPoints .|> length # TODO upgrade to multiscale # start with random selection of labels - best_labels = [rand(1:n) for n in Ns] + latest_labels = [rand(1:n) for n in Ns] # pick the next leave-out proposal for _ in MC, O in 1:d # select a label from the not-selected proposal densities sublabels = Tuple{Int,Int}[] for s in setdiff(1:d, O) - slbl = best_labels[s] + slbl = latest_labels[s] push!(sublabels, (s,slbl)) end # prop = proposals[s] # TODO upgrade to map and tuples - components = map(sl->getKernel(proposals[sl[1]], sl[2], true), sublabels) + components = map(sl->getKernelLeaf(proposals[sl[1]], sl[2], true), sublabels) # calc product of Gaussians from currently selected LOO-proposals newO = calcProductGaussians(M, [components...]) # evaluate new sampling weights of points in out component + # NOTE getPoints returns the sorted (permuted) list of data evat = getPoints(proposals[O]) # FIXME how should partials be handled here? smw = zeros(length(evat)) # FIXME, use multipoint evaluation such as NN (not just one point at a time) @@ -600,19 +669,22 @@ function sampleProductSeqGibbsLabel( p = Categorical(smw) # update label-distribution of out-proposal from product of selected LOO-proposal components - best_labels[O] = rand(p) + latest_labels[O] = rand(p) end + # # recursively sample a layer deeper for each selected label, or fix that sample if that label is a leaf kernel + # for (i,l) in enumerate(latest_labels) + + # end + # - - return best_labels + return latest_labels end function sampleProductSeqGibbsLabels( M::AbstractManifold, proposals::AbstractVector, - treeLevel::Int = 1, # reserved for future use MC = 3, N::Int = round(Int, mean(length.(getPoints.(proposals)))) ) @@ -621,7 +693,7 @@ function sampleProductSeqGibbsLabels( posterior_labels = Vector{NTuple{d,Int}}(undef,N) for i in 1:N - posterior_labels[i] = tuple(sampleProductSeqGibbsLabel(M,proposals,treeLevel,MC)...) + posterior_labels[i] = tuple(sampleProductSeqGibbsLabel(M,proposals,MC)...) end posterior_labels @@ -638,10 +710,11 @@ function calcProductKernelLabels( post = [] for lbs in lbls + # FIXME different tree or leaf kernels would need different lists props = MvNormalKernel[] for (i,lb) in enumerate(lbs) # selection of labels was done against sorted list of particles, hence false - push!(props, getKernel(proposals[i],lb,false)) + push!(props, getKernelLeaf(proposals[i],lb,false)) end push!(post,calcProductGaussians(M,props)) end diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index d9e80a1..eca5d87 100644 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -50,7 +50,7 @@ M = TranslationGroup(2) r_CC, R, pidx, r_CV = testEigenCoords(α); ax_CCp, mask, knl = splitPointsEigen(M, r_CC) @test sum(mask) == (length(r_CC) ÷ 2) -@test knl isa MvNormal +@test knl isa ApproxManifoldProducts.MvNormalKernel Mr = SpecialOrthogonal(2) @test isapprox( α, vee(Mr, Identity(Mr), log_lie(Mr, R))[1] ; atol=0.1) @@ -113,6 +113,29 @@ end @testset "ManellicTree construction 1D" begin ## +M = TranslationGroup(1) +# already sorted list +pts = [[1.],[2.],[4.],[7.],[11.],[16.],[22.]] +bw = [1.0] +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) + +@test 7 == length( intersect( mtree.segments[1], Set(1:7)) ) +@test 4 == length( intersect( mtree.segments[2], Set(1:4)) ) +@test 3 == length( intersect( mtree.segments[3], Set(5:7)) ) +@test 2 == length( intersect( mtree.segments[4], Set(1:2)) ) +@test 2 == length( intersect( mtree.segments[5], Set(3:4)) ) +@test 2 == length( intersect( mtree.segments[6], Set(5:6)) ) + +@test isapprox( mean(M,pts), mean(mtree.tree_kernels[1]); atol=1e-6) +@test isapprox( mean(M,pts[1:4]), mean(mtree.tree_kernels[2]); atol=1e-6) +@test isapprox( mean(M,pts[5:7]), mean(mtree.tree_kernels[3]); atol=1e-6) +@test isapprox( mean(M,pts[1:2]), mean(mtree.tree_kernels[4]); atol=1e-6) +@test isapprox( mean(M,pts[3:4]), mean(mtree.tree_kernels[5]); atol=1e-6) +@test isapprox( mean(M,pts[5:6]), mean(mtree.tree_kernels[6]); atol=1e-6) + + +## additional test datasets + function testMDEConstr( pts::AbstractVector{<:AbstractVector{<:Real}}, permref = sortperm(pts, by=s->getindex(s,1)); @@ -183,6 +206,7 @@ for i in 1:10 testMDEConstr( _pts; lseg=1:4,rseg=5:8 ) end + ## end @@ -457,6 +481,31 @@ g = ApproxManifoldProducts.calcProductGaussians(M, [g1; g2]) end +# @testset "Test utility functions for multi-scale product sampling" begin +# ## + +# M = TranslationGroup(1) + +# pts = [randn(1).-1 for _ in 1:3] +# p1 = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[0.1;;], kernel=ApproxManifoldProducts.MvNormalKernel) + +# @test 1 == length(ApproxManifoldProducts.getKernelsTreeLevelIdxs(p1, 1)) +# @test 2 == length(ApproxManifoldProducts.getKernelsTreeLevelIdxs(p1, 2)) +# @test 4 == length(ApproxManifoldProducts.getKernelsTreeLevelIdxs(p1, 3)) + +# @test 64 == length(ApproxManifoldProducts.getKernelsTreeLevelIdxs(p1, 7)) +# @test 128 == length(ApproxManifoldProducts.getKernelsTreeLevelIdxs(p1, 8)) + +# # @enter +# ApproxManifoldProducts.getKernelsTreeLevelIdxs(p1, 2) +# ApproxManifoldProducts.getKernelsTreeLevelIdxs(p1, 3) + + + +# ## +# end + + @testset "Product of two Manellic beliefs, Sequential Gibbs" begin ## @@ -468,6 +517,7 @@ p1 = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[0.1;;], kerne pts = [randn(1).+1 for _ in 1:128] p2 = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=[0.1;;], kernel=ApproxManifoldProducts.MvNormalKernel) +## lbls = ApproxManifoldProducts.sampleProductSeqGibbsLabels(M, [p1; p2]) @@ -480,6 +530,7 @@ mtr = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw, kernel=Appro @test isapprox( 0, mean(mtr.tree_kernels[1])[1]; atol=0.75) + ## end From 0f4b8db768a49a413579515d6e01cc794392efb0 Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Wed, 24 Apr 2024 18:23:35 -0400 Subject: [PATCH 08/31] Manellic products are now multiscale (also for unbalanced trees) (#282) * refactor prods w label_sets * use BT labels toward multiscale sampling * multiscale functional, untested, suspect permute issue * refac manellic util fncs toward multiscale tests * refac manellic left right index * manellic multiscale prod cov_cont functional, but not yet * manellic multiscale product working and few tests --- src/ApproxManifoldProducts.jl | 2 +- src/CommonUtils.jl | 27 +- src/entities/ManellicTree.jl | 1 + src/services/KernelEval.jl | 2 +- src/services/ManellicTree.jl | 521 +++++++++++++++++++----------- test/manellic/testManellicTree.jl | 72 ++++- 6 files changed, 405 insertions(+), 220 deletions(-) diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index b1db91b..c6e8b8f 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -29,7 +29,7 @@ using Distributions import Random: rand -import Base: *, isapprox, convert, show, eltype +import Base: *, isapprox, convert, show, eltype, length import LinearAlgebra: rotate!, det import Statistics: mean, std, cov, var, entropy import KernelDensityEstimate: getPoints, getBW, evalAvgLogL, entropy, evaluate diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index 715a965..a13e0e9 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -116,6 +116,10 @@ Notes - calc lambdas first and use to calculate mean product second. - https://ccrma.stanford.edu/~jos/sasp/Product_Two_Gaussian_PDFs.html - Pennec, X. Intrinsic Statistics on Riemannian Manifolds: Basic Tools for Geometric Measurements, HAL Archive, 2011, Inria, France. + +DevNotes: +- FIXME is parallel transport needed as products involve covariances from different tangent spaces? +- TODO avoid recomputing covariance matrix inverses all the time """ function calcProductGaussians( M::AbstractManifold, @@ -166,7 +170,14 @@ calcProductGaussians( ) where {N,P} = calcProductGaussians(M, μ_, nothing; dim, Λ_=Λ_ ) +""" + $SIGNATURES + +EXPERIMENTAL: On-manifold product of Gaussians. +DevNotes +- FIXME is parallel transport needed when multiplying with covariances from difffent tangent spaces? +""" function calcProductGaussians( M::AbstractManifold, comps::AbstractVector{<:MvNormalKernel}, @@ -175,23 +186,11 @@ function calcProductGaussians( μ_ = mean.(comps) Σ_ = cov.(comps) + # FIXME is parallel transport needed here for covariances from different tangent spaces? + _μ, _Σ = calcProductGaussians(M, μ_, Σ_) return MvNormalKernel(_μ, _Σ) - # d = manifold_dimension(M) - # # TODO avoid recomputing covariance matrix inverses all the time - # _Sigma2 = zeros(d,d) - # _Sig2mu = zeros(d) - # for c in comps - # concentration = inv(cov(c)) - # _Sigma2 += concentration - # _Sig2mu += concentration * mean(c) - # end - - # Sigma2 = inv(_Sigma2) - # mu = Sigma2 * _Sig2mu - - # return MvNormalKernel(mu, Sigma2) end diff --git a/src/entities/ManellicTree.jl b/src/entities/ManellicTree.jl index 5398ebe..3febab1 100644 --- a/src/entities/ManellicTree.jl +++ b/src/entities/ManellicTree.jl @@ -31,5 +31,6 @@ struct ManellicTree{M,D<:AbstractVector,N,HL,HT} # workaround to overcome bug for StaticArrays `isdefined() != false` issue _workaround_isdef_treekernel::Set{Int} + _workaround_isdef_leafkernel::Set{Int} end diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index 76ed321..88a3249 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -7,7 +7,7 @@ function projectSymPosDef(c::AbstractMatrix) issymmetric(_c) ? _c : project(SymmetricPositiveDefinite(s[1]),_c,_c) end -# FIXME, REMOVE TYPE DISPLACEMENT +# FIXME ON FIRE, REMOVE TYPE DISPLACEMENT Base.eltype(mt::MvNormalKernel) = eltype(mt.p) function MvNormalKernel( diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 2c74e39..5baddbd 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -9,6 +9,8 @@ # end # end +Base.length(::ManellicTree{M,D,N}) where {M,D,N} = N + getPoints( mt::ManellicTree @@ -19,15 +21,20 @@ getWeights( ) = view(mt.weights, mt.permute) +# _getleft(i::Integer, N) = 2*i + (2*i < N ? 0 : 1) +# _getright(i::Integer, N) = _getleft(i,N) + 1 + + # either leaf or tree kernel, if larger than N leftIndex( - ::ManellicTree, + mt::ManellicTree, krnIdx::Int=1 -) = 2*krnIdx +) = 2*krnIdx + (2*krnIdx < length(mt) ? 0 : 1) + rightIndex( - ::ManellicTree, + mt::ManellicTree, krnIdx::Int -) = 2*krnIdx+1 +) = leftIndex(mt,krnIdx) + 1 """ @@ -42,6 +49,100 @@ getKernelLeaf( permuted::Bool = true ) = mt.leaf_kernels[permuted ? mt.permute[i] : i] + + +""" + $SIGNATURES + +Return leaf kernels as tree kernel types, using regular `[1..N]` indexing]. + +Notes: +- use `permute=true` (default) for sorted index retrieval. +""" +getKernelLeafAsTreeKer( + mtr::ManellicTree{M,D,N,HL,HT}, + idx::Int, + permuted::Bool=false +) where {M,D,N,HL,HT} = convert(HT,getKernelLeaf(mtr, (idx-1) % N + 1, permuted)) + +""" + $SIGNATURES + +Return kernel from tree by binary tree index, and convert leaf kernels to tree kernel types if necessary. + +See also: [`getKernelLeafAsTreeKer`](@ref) +""" +function getKernelTree( + mtr::ManellicTree{M,D,N,HL,HT}, + currIdx::Int, + # must return sorted given name signature "Tree" + permuted = false, + cov_continuation::Bool = false, +) where {M,D,N,HL,HT} + # + + + # BinaryTree (BT) index goes from root=1 to largest leaf 2*N + if currIdx < N + # cov_continuation correction so that we may build trees with sensible convariance to bandwidth transition from root to leaf + raw_ker = mtr.tree_kernels[currIdx] + if cov_continuation + # depth of this index + ances_depth = floor(Int, log2(currIdx)) + # how many leaf offsp + offsp_depth = log2(length(mtr.segments[currIdx])) + # get approx continuous depth fraction of this index + λ = (ances_depth) / (ances_depth + offsp_depth) + # mean bandwidth of all leaf children + leafIdxs = mtr.segments[currIdx] .|> s->findfirst(==(s),mtr.permute) + leafIdxs .+= N + bws = [cov(getKernelTree(mtr,lidx,false)) for lidx in leafIdxs] + # FIXME is a parallel transport needed between different kernel covariances that each exist in different tangent spaces + mean_bw = mean(bws) + # corrected cov varies from root (only Monte Carlo cov est) to leaves (only selected bandwdith) + nC = (1-λ)*cov(raw_ker) + λ*mean_bw + # return a new kernel with cov_continuation, of tree kernel type + kernelType = getfield(ApproxManifoldProducts,HT.name.name) + kernelType(mean(raw_ker), nC, mtr.weights[currIdx]) + else + raw_ker + end + else + getKernelLeafAsTreeKer(mtr, currIdx, permuted) + end +end + + +function exists_BTLabel( + mt::ManellicTree{M,D,N}, + idx::Int +) where {M,D,N} + # check for existence in tree or leaves + eset = if idx < N + mt._workaround_isdef_treekernel + else + mt._workaround_isdef_leafkernel + end + + # return existence + return idx in eset +end + +function isLeaf_BTLabel( + mt::ManellicTree{M,D,N}, + idx::Int +) where {M,D,N} + if exists_BTLabel(mt, leftIndex(mt,idx)) + return false + elseif exists_BTLabel(mt, rightIndex(mt,idx)) + # TODO likely not needed to check for right child existence + return false + else + return true + end +end + + uniWT(mt::ManellicTree) = 1===length(union(diff(getWeights(mt)))) function uniBW( @@ -58,6 +159,7 @@ function uniBW( return true end + function Base.show( io::IO, mt::ManellicTree{M,D,N,TK} @@ -154,6 +256,7 @@ end + # covariance function eigenCoords( f_CVp::AbstractMatrix @@ -270,13 +373,10 @@ function splitPointsEigen( end -_getleft(i::Integer) = 2*i -_getright(i::Integer) = 2*i + 1 - function buildTree_Manellic!( mtree::ManellicTree{MT,D,N}, - index::Integer, # tree node root=1,left=2n,right=left+1 + index::Integer, # tree node root=1,left=2n+corr,right=left+1 low::Integer, # bottom index of segment high::Integer; # top index of segment; kernel = MvNormal, @@ -290,6 +390,7 @@ function buildTree_Manellic!( _kernel_bw = _legacybw(kernel_bw) + # terminate recursive tree build when all necessary tree kernels have been built if N <= index return mtree end @@ -314,8 +415,8 @@ function buildTree_Manellic!( # @info "BUILD" index low sum(mask) mid_idx high _getleft(index) _getright(index) - lft = mid_idx <= low ? low : _getleft(index) - rgt = high <= mid_idx+1 ? high : _getright(index) + lft = mid_idx <= low ? low : leftIndex(mtree, index) + rgt = high <= mid_idx+1 ? high : rightIndex(mtree, index) if leaf_size < npts if lft != low # mid_idx @@ -328,11 +429,10 @@ function buildTree_Manellic!( end end - # set HyperEllipse at this level in tree - # FIXME, replace with just the kernel choice, not hyper such and such needed? if index < N _knl = convert(eltype(mtree.tree_kernels), knl) - mtree.tree_kernels[index] = _knl # HyperEllipse(knl.μ, knl.Σ.mat) + # FIXME use consolidate getKernelTree instead + mtree.tree_kernels[index] = _knl push!(mtree._workaround_isdef_treekernel, index) mtree.segments[index] = Set(ido) end @@ -356,9 +456,9 @@ DevNotes: function buildTree_Manellic!( M::AbstractManifold, r_PP::AbstractVector{P}; # vector of points referenced to the r_frame - len = length(r_PP), - weights::AbstractVector{<:Real} = ones(len).*(1/len), - kernel = MvNormal, + N = length(r_PP), + weights::AbstractVector{<:Real} = ones(N).*(1/N), + kernel = MvNormalKernel, kernel_bw = nothing, # TODO ) where {P <: AbstractArray} # @@ -374,11 +474,6 @@ function buildTree_Manellic!( _legacybw(::Nothing) = CV lCV = _legacybw(kernel_bw) - # lCV = if isnothing(kernel_bw) - # CV - # else - # kernel_bw - # end lknlT = kernel( r_PP[1], @@ -388,24 +483,26 @@ function buildTree_Manellic!( # kernel scale # leaf kernels - lkern = SizedVector{len,lknlT}(undef) - for i in 1:len + lkern = SizedVector{N,lknlT}(undef) + _workaround_isdef_leafkernel = Set{Int}() + for i in 1:N lkern[i] = kernel(r_PP[i], lCV) + push!(_workaround_isdef_leafkernel, i + N) end - mtree = ManellicTree( M, r_PP, - MVector{len,Float64}(weights), - MVector{len,Int}(1:len), - lkern, # MVector{len,lknlT}(undef), - SizedVector{len,tknlT}(undef), - # SizedVector{len,tknlT}(undef), - SizedVector{len,Set{Int}}(undef), - MVector{len,Int}(undef), - MVector{len,Int}(undef), - Set{Int}() + MVector{N,Float64}(weights), + MVector{N,Int}(1:N), + lkern, # MVector{N,lknlT}(undef), + SizedVector{N,tknlT}(undef), + # SizedVector{N,tknlT}(undef), + SizedVector{N,Set{Int}}(undef), + MVector{N,Int}(undef), + MVector{N,Int}(undef), + Set{Int}(), + _workaround_isdef_leafkernel ) # @@ -413,7 +510,7 @@ function buildTree_Manellic!( mtree, 1, # start at root 1, # spanning all data - len; # to end of data + N; # to end of data kernel, kernel_bw ) @@ -428,6 +525,44 @@ function buildTree_Manellic!( return tosort_leaves end + +""" + $SIGNATURES + +For Manellic tree parent kernels, what is the 'smallest' and 'biggest' covariance. + +Notes: +- Thought about `det` for covariance volume but long access of pancake (smaller volume) is not minimum compared to circular covariance. +""" +function getBandwidthSearchBounds( + mtree::ManellicTree +) + upper = cov(mtree.tree_kernels[1]) + + #FIXME isdefined does not work as expected for mtree.tree_kernels, so using length-1 for now + # this will break if number of points is not a power of 2. + kernels_diag = map(1:length(mtree.tree_kernels)-1) do i + # FIXME use cosnolidated getKernelTree instead + diag(cov(mtree.tree_kernels[i])) + end + lower_diag = minimum(reduce(hcat, kernels_diag), dims=2) + + # floors make us feel safe, but hurt when faceplanting + lower_diag = maximum( + hcat( + lower_diag, + 1e-8*ones(length(lower_diag)) + ), + dims=2 + )[:] + + # Give back lower as diagonal only covariance matrix + lower = diagm(lower_diag) + + return lower, upper +end + + """ $SIGNATURES @@ -443,8 +578,7 @@ DevNotes: """ function evaluate( mt::ManellicTree{M,D,N,HL}, - p, - # bw_scl::Real = 1, + pt, LOO::Bool = false, ) where {M,D,N,HL} # force function barrier, just to be sure dyndispatch is limited @@ -457,11 +591,13 @@ function evaluate( sumval = 0.0 # FIXME, brute force for loop for (i,t) in enumerate(pts) - if !LOO || !isapprox(p, t) # FIXME change isapprox to on-manifold version + # FIXME change isapprox to on-manifold version + if !LOO || !isapprox(pt, t) # FIXME, is this assuming length(pts) and length(mt.leaf_kernels) are the same? + # FIXME use consolidated getKernelLeaf instead ekr = mt.leaf_kernels[i] # TODO remember special handling for partials in the future - oneval = mt.weights[i] * evaluate(mt.manifold, ekr, p) + oneval = mt.weights[i] * evaluate(mt.manifold, ekr, pt) oneval *= !LOO ? 1 : 1/(1-w[i]) sumval += oneval end @@ -471,10 +607,45 @@ function evaluate( end +""" + $SIGNATURES + +Return vector of weights of evaluated proposal label points against density. + +DevNotes: +- TODO should evat points be of equal weights? If multiscale sampling goes down unbalanced trees? +- FIXME how should partials be handled here? +- FIXME, use multipoint evaluation such as NN (not just one point at a time) +""" +function evaluateDensityAtPoints( + M::AbstractManifold, + density, + eval_at_points, + normalize::Bool = true +) + # evaluate new sampling weights of points in out component + + # vector for storing resulting weights + smw = zeros(length(eval_at_points)) + for (i,ev) in enumerate(eval_at_points) + # single kernel evaluation + smw[i] = evaluate(M, density, ev) + # δc = distanceMalahanobisCoordinates(M,tmp_product,ev) + end + + if normalize + smw ./= sum(smw) + end + + # return weights + return smw +end + + + function expectedLogL( mt::ManellicTree{M,D,N}, epts::AbstractVector, - # bw_scl::Real = 1, LOO::Bool = false ) where {M,D,N} T = Float64 @@ -495,231 +666,187 @@ function expectedLogL( -Inf else w'*(log.(eL)) - # return mean(log.(eL)) + # return mean(log.(eL)) #? end end -# pathelogical case return -Inf entropy( mt::ManellicTree, ) = -expectedLogL(mt, getPoints(mt), true) -# leaveOneOutLogL( -# mt::ManellicTree, -# bw_scl::Real = 1, -# ) = entropy(mt, bw_scl) - (mt::ManellicTree)( evalpt::AbstractArray, ) = evaluate(mt, evalpt) + """ $SIGNATURES - -For Manellic tree parent kernels, what is the 'smallest' and 'biggest' covariance. -Notes: -- Thought about `det` for covariance volume but long access of pancake (smaller volume) is not minimum compared to circular covariance. +Calculate one product of proposal kernels, as defined BTLabels. """ -function getBandwidthSearchBounds( - mtree::ManellicTree +function calcProductKernelBTLabels( + M::AbstractManifold, + proposals::AbstractVector, + labels_sampled, + LOOidx::Union{Int, Nothing} = nothing, + gibbsSeq = 1:length(proposals); + permute::Bool = true # true because signature is BTLabels ) - upper = cov(mtree.tree_kernels[1]) - - #FIXME isdefined does not work as expected for mtree.tree_kernels, so using length-1 for now - # this will break if number of points is not a power of 2. - kernels_diag = map(1:length(mtree.tree_kernels)-1) do i - diag(cov(mtree.tree_kernels[i])) + # select a density label from the other proposals + prop_and_label = Tuple{Int,Int}[] + for s in setdiff(gibbsSeq, isnothing(LOOidx) ? Int[] : Int[LOOidx;]) + # tuple of which leave-one-out-proposal and its new latest label selection + push!(prop_and_label, (s, labels_sampled[s])) end - lower_diag = minimum(reduce(hcat, kernels_diag), dims=2) - - # floors make us feel safe, but hurt when faceplanting - lower_diag = maximum( - hcat( - lower_diag, - 1e-8*ones(length(lower_diag)) - ), - dims=2 - )[:] - - # Give back lower as diagonal only covariance matrix - lower = diagm(lower_diag) + # get raw kernels from tree, also as tree_kernel type + # NOTE DO COVARIANCE CONTINUATION CORRECTION FOR DEPTH OF TREE KERNELS + components = map(pr_lb->getKernelTree(proposals[pr_lb[1]], pr_lb[2], permute, true), prop_and_label) - return lower, upper + # TODO upgrade to tuples + return calcProductGaussians(M, [components...]) end -# ## Pseudo code - -# X1 = fg[:x1] # Position{2} - -# # prob density -# x1 = X1([12.3;0.7]) -# x1 = X1([-8.8;0.7]) - -# X1_a = approxConvBelief(dfg, f1, :x1) -# X1_b = approxConvBelief(dfg, f2, :x1) - -# _X1_ = X1_a*X1_b - -## WIP Sequential Gibbs Product development +function calcProductKernelsBTLabels( + M::AbstractManifold, + proposals::AbstractVector, + N_lbl_sets::AbstractVector{<:NTuple}, + permute::Bool = true # true because signature is BTLabels +) + # + T = typeof(getKernelTree(proposals[1],1)) + post = Vector{T}(undef, length(N_lbl_sets)) -getKernelLeafAsTreeKer(mtr::ManellicTree{M,D,N,HL,HT}, idx::Int, permuted::Bool=true) where {M,D,N,HL,HT} = convert(HT,getKernelLeaf(mtr, idx % N, permuted)) - -function getKernelTree( - mtr::ManellicTree{M,D,N}, - currIdx::Int, -) where {M,D,N} - # must return sorted given name signature "Tree" - permuted = true - - if currIdx < N - return mtr.tree_kernels[currIdx] - else - return getKernelLeafAsTreeKer(mtr, currIdx, permuted) + for (i,lbs) in enumerate(N_lbl_sets) + post[i] = calcProductKernelBTLabels(M, proposals, lbs; permute) end -end + return post +end -# function getKernelsTreeLevelIdxs( -# mtr::ManellicTree{M,D,N}, -# level::Int, -# currIdx::Int = 1 -# ) where {M,D,N} - -# # go to children if idx level too high -# _idxlevel(idx) = floor(Int,log2(idx)) + 1 -# _exists(_i) = _i < N ? (_i in mtr._workaround_isdef_treekernel) : true # N from numPoints anyway # isdefined(mtr.leaf_kernels,_i) - -# #traverse tree -# if level == _idxlevel(currIdx) && _exists(currIdx) -# return Int[currIdx;] -# end - -# @warn "getKernelsTreeLevelIdxs still has corner case issues when tree_kernel[N]" maxlog=10 -# # explore left and right -# return vcat( -# getKernelsTreeLevelIdxs(mtr,level,_getleft(currIdx)), -# getKernelsTreeLevelIdxs(mtr,level,_getright(currIdx)) -# ) -# end +function generateLabelPoolRecursive( + proposals::AbstractVector{<:ManellicTree}, + labels_sampled::AbstractVector{<:Integer} +) + # NOTE at top of tree, selections will be [1,1] + child_label_pools = Vector{Vector{Int}}() + + # Are all selected labels leaves? + all_leaves = true + for _ in 1:length(proposals) + push!(child_label_pools, Vector{Int}()) + end + for (o,idx) in enumerate(labels_sampled) + isleaf = true + # add interval of left and right children for next scale label sampling + if exists_BTLabel(proposals[o], leftIndex(proposals[o], idx)) + push!(child_label_pools[o], leftIndex(proposals[o], idx)) + isleaf = false + end + if exists_BTLabel(proposals[o], rightIndex(proposals[o], idx)) + push!(child_label_pools[o], rightIndex(proposals[o], idx)) + isleaf = false + end + all_leaves &= isleaf + if isleaf + push!(child_label_pools[o], idx) + end + end -# function getKernelsTreeLevel( -# mtr::ManellicTree{M,D,N}, -# level::Int -# ) where {M,D,N} -# # kernels of that level -# levelIdxs = getKernelsTreeLevelIdxs(mtr,level) -# getKernelTree.(Ref(mtr),levelIdxs) -# end + return child_label_pools, all_leaves +end +""" + $SIGNATURES -function sampleProductSeqGibbsLabel( +Notes: +- Advise, 2<=MC to ensure multiscale works during decent transitions (TBD obsolete requirement) +- To force sequential Gibbs on leaves only, use: + `label_pools = [[(length(getPoints(prop))+1):(2*length(getPoints(prop)));] for prop in proposals]` +""" +function sampleProductSeqGibbsBTLabel( M::AbstractManifold, - proposals::AbstractVector, + proposals::AbstractVector{<:ManellicTree}, MC = 3, + # pool of sampleable labels + label_pools::Vector{Vector{Int}}= [[1:1;] for _ in proposals], + labels_sampled::Vector{Int} = ones(Int, length(proposals)); + # multiscale_parents = nothing; + MAX_RECURSE_DEPTH::Int = 24, # 2^24 is so deep ) # # how many incoming proposals d = length(proposals) + gibbsSeq = 1:d - ## TODO sample at multiscale levels on the tree, starting at treeLevel=1 - - # how many points per proposal at this depth level of the belief tree - Ns = proposals .|> getPoints .|> length - - # TODO upgrade to multiscale - # start with random selection of labels - latest_labels = [rand(1:n) for n in Ns] - # pick the next leave-out proposal - for _ in MC, O in 1:d - # select a label from the not-selected proposal densities - sublabels = Tuple{Int,Int}[] - for s in setdiff(1:d, O) - slbl = latest_labels[s] - push!(sublabels, (s,slbl)) - end - # prop = proposals[s] + for _ in 1:MC, O in gibbsSeq + # on first pass labels_sampled come from parent-recursive as part of multi-scale (i.e. pre-homotopy) operations + # calc product of Gaussians from currently selected \LOO-proposals + tmp_product = calcProductKernelBTLabels(M, proposals, labels_sampled, O, gibbsSeq; permute=false) - # TODO upgrade to map and tuples - components = map(sl->getKernelLeaf(proposals[sl[1]], sl[2], true), sublabels) - - # calc product of Gaussians from currently selected LOO-proposals - newO = calcProductGaussians(M, [components...]) - - # evaluate new sampling weights of points in out component - # NOTE getPoints returns the sorted (permuted) list of data - evat = getPoints(proposals[O]) # FIXME how should partials be handled here? - smw = zeros(length(evat)) - # FIXME, use multipoint evaluation such as NN (not just one point at a time) - for (i,ev) in enumerate(evat) - smw[i] = evaluate(M, newO, ev) - # δc = distanceMalahanobisCoordinates(M,newO,ev) - end - - smw ./= sum(smw) + # evaluate new weights for set of points from LOO proposal means + eval_at_points = [mean(getKernelTree(proposals[O], i, false)) for i in label_pools[O]] + smw = evaluateDensityAtPoints(M, tmp_product, eval_at_points, true) # TBD: smw = evaluate(tmp_product, ) - # smw = evaluate(newO, ) - p = Categorical(smw) - # update label-distribution of out-proposal from product of selected LOO-proposal components - latest_labels[O] = rand(p) + p = Categorical(smw) + labels_sampled[O] = label_pools[O][rand(p)] end + + # construct new label pool for children in multiscale + child_label_pools, all_leaves = generateLabelPoolRecursive(proposals, labels_sampled) + + # @info "WHY STOP" child_label_pools all_leaves + + # recursively call sampling down the multiscale tree ("pyramid") -- aka homotopy + # limit recursion to MAX_RECURSE_DEPTH + if 0 Date: Tue, 30 Apr 2024 03:37:30 -0400 Subject: [PATCH 09/31] add dev manikde!_manellic for 1D only (#284) * add dev manikde!_manellic for 1D only * test fix --- src/CommonUtils.jl | 12 ++++--- src/services/KernelEval.jl | 2 +- src/services/ManellicTree.jl | 7 ++-- src/services/ManifoldKernelDensity.jl | 46 +++++++++++++++++++++++++++ test/manellic/testManellicTree.jl | 14 +++++--- 5 files changed, 68 insertions(+), 13 deletions(-) diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index a13e0e9..a0f7574 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -123,10 +123,10 @@ DevNotes: """ function calcProductGaussians( M::AbstractManifold, - μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P + μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P (actually on-manifold) Σ_::Union{Nothing,<:AbstractVector{S},<:NTuple{N,S}}; dim::Integer=manifold_dimension(M), - Λ_ = inv.(Σ_), + Λ_ = inv.(Σ_), # TODO these probably need to be transported to common tangent space `u0` -- FYI @Affie 24Q2 ) where {N,P,S<:AbstractMatrix{<:Real}} # # calc sum of covariances @@ -176,15 +176,17 @@ calcProductGaussians( EXPERIMENTAL: On-manifold product of Gaussians. DevNotes +- CHECK make sure this product is properly on manifold, Manifolds.jl likely already has solutions: + - https://juliamanifolds.github.io/Manifolds.jl/stable/features/distributions.html - FIXME is parallel transport needed when multiplying with covariances from difffent tangent spaces? """ function calcProductGaussians( M::AbstractManifold, comps::AbstractVector{<:MvNormalKernel}, ) - # - μ_ = mean.(comps) - Σ_ = cov.(comps) + # CHECK this should be on-manifold for points + μ_ = mean.(comps) # This is a ArrayPartition which IS DEFINITELY ON MANIFOLD (we dispatch on mean) + Σ_ = cov.(comps) # on tangent # FIXME is parallel transport needed here for covariances from different tangent spaces? diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index 88a3249..632da92 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -30,7 +30,7 @@ Statistics.std(m::MvNormalKernel) = sqrt(cov(m)) function evaluate( M::AbstractManifold, ekr::MvNormalKernel, - p + p # on manifold point ) # dim = manifold_dimension(M) diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 5baddbd..802c49f 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -98,7 +98,7 @@ function getKernelTree( leafIdxs .+= N bws = [cov(getKernelTree(mtr,lidx,false)) for lidx in leafIdxs] # FIXME is a parallel transport needed between different kernel covariances that each exist in different tangent spaces - mean_bw = mean(bws) + mean_bw = mean(bws) # FIXME upgrade to on-manifold mean # corrected cov varies from root (only Monte Carlo cov est) to leaves (only selected bandwdith) nC = (1-λ)*cov(raw_ker) + λ*mean_bw # return a new kernel with cov_continuation, of tree kernel type @@ -573,6 +573,7 @@ DevNotes: - use geometric computing for faster evaluation - Dual tree evaluations - Holmes, M.P., Gray, A.G. and Isbell Jr, C.L., 2010. Fast kernel conditional density estimation: A dual-tree Monte Carlo approach. Computational statistics & data analysis, 54(7), pp.1707-1718. + - Curtin, R., March, W., Ram, P., Anderson, D., Gray, A. and Isbell, C., 2013, May. Tree-independent dual-tree algorithms. In International Conference on Machine Learning (pp. 1435-1443). PMLR. - Fast kernels - Parallel transport shortcuts? """ @@ -768,6 +769,8 @@ Notes: - Advise, 2<=MC to ensure multiscale works during decent transitions (TBD obsolete requirement) - To force sequential Gibbs on leaves only, use: `label_pools = [[(length(getPoints(prop))+1):(2*length(getPoints(prop)));] for prop in proposals]` +- Taken from: Sudderth, E.B., Ihler, A.T., Isard, M., Freeman, W.T. and Willsky, A.S., 2010. + Nonparametric belief propagation. Communications of the ACM, 53(10), pp.95-103. """ function sampleProductSeqGibbsBTLabel( M::AbstractManifold, @@ -802,8 +805,6 @@ function sampleProductSeqGibbsBTLabel( # construct new label pool for children in multiscale child_label_pools, all_leaves = generateLabelPoolRecursive(proposals, labels_sampled) - # @info "WHY STOP" child_label_pools all_leaves - # recursively call sampling down the multiscale tree ("pyramid") -- aka homotopy # limit recursion to MAX_RECURSE_DEPTH if 0_cost(pts,s^2), + lcov[1], ucov[1], Optim.GoldenSection() + ) + best_cov = [Optim.minimizer(res);;] + + # return tree with correct bandwidth + # TODO avoid tree rebuild somehow + manikde!( + M, + pts; + bw=best_cov, + belmodel = (a,b,aF,dF) -> ApproxManifoldProducts.buildTree_Manellic!( + M, + pts; + kernel_bw=b, + kernel=AMP.MvNormalKernel + ) + ) +end + ## ========================================================================================== ## a few utilities diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 1d8ae3d..8a50e88 100644 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -418,12 +418,18 @@ end # TODO -# @testset "Manellic tree bandwidth optimize n-dim RLM" begin -# ## +@testset "Manellic tree all up construction with bandwith optimization" begin +## -# ## -# end +M = TranslationGroup(1) +# pts = [[0.;],[0.1],[0.2;],[0.3;]] +pts = [1*randn(1) for _ in 1:64] + +mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) + +## +end From 5b1a9ff140dce63b9e19f87d69f614810f7a51bf Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Tue, 30 Apr 2024 18:59:03 -0400 Subject: [PATCH 10/31] improved manikde!_manellic construction (#285) * improved manikde!_manellic construction * rm extraneous fields and code ManellicTree --- src/entities/ManellicTree.jl | 6 ++-- src/services/KernelEval.jl | 4 +-- src/services/ManellicTree.jl | 49 +++++++++++++++++++-------- src/services/ManifoldKernelDensity.jl | 34 +++++++++---------- 4 files changed, 54 insertions(+), 39 deletions(-) diff --git a/src/entities/ManellicTree.jl b/src/entities/ManellicTree.jl index 3febab1..557f3fb 100644 --- a/src/entities/ManellicTree.jl +++ b/src/entities/ManellicTree.jl @@ -26,11 +26,11 @@ struct ManellicTree{M,D<:AbstractVector,N,HL,HT} leaf_kernels::SizedVector{N,HL} tree_kernels::SizedVector{N,HT} segments::SizedVector{N,Set{Int}} - left_idx::MVector{N,Int} - right_idx::MVector{N,Int} + # left_idx::MVector{N,Int} + # right_idx::MVector{N,Int} # workaround to overcome bug for StaticArrays `isdefined() != false` issue - _workaround_isdef_treekernel::Set{Int} _workaround_isdef_leafkernel::Set{Int} + _workaround_isdef_treekernel::Set{Int} end diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index 632da92..c65145b 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -7,9 +7,6 @@ function projectSymPosDef(c::AbstractMatrix) issymmetric(_c) ? _c : project(SymmetricPositiveDefinite(s[1]),_c,_c) end -# FIXME ON FIRE, REMOVE TYPE DISPLACEMENT -Base.eltype(mt::MvNormalKernel) = eltype(mt.p) - function MvNormalKernel( μ::AbstractVector, Σ::AbstractArray, @@ -26,6 +23,7 @@ Statistics.mean(m::MvNormalKernel) = m.μ # mean(m.p) # m.p.μ Statistics.cov(m::MvNormalKernel) = cov(m.p) # note also about m.sqrt_iΣ Statistics.std(m::MvNormalKernel) = sqrt(cov(m)) +updateKernelBW(k::MvNormalKernel,_bw) = (p=MvNormal(_bw); MvNormalKernel(;μ=k.μ,p,weight=k.weight)) function evaluate( M::AbstractManifold, diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 802c49f..b54245a 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -413,8 +413,6 @@ function buildTree_Manellic!( npts = high - low + 1 mid_idx = low + sum(imask) - 1 - # @info "BUILD" index low sum(mask) mid_idx high _getleft(index) _getright(index) - lft = mid_idx <= low ? low : leftIndex(mtree, index) rgt = high <= mid_idx+1 ? high : rightIndex(mtree, index) @@ -431,7 +429,7 @@ function buildTree_Manellic!( if index < N _knl = convert(eltype(mtree.tree_kernels), knl) - # FIXME use consolidate getKernelTree instead + # set tree kernel mtree.tree_kernels[index] = _knl push!(mtree._workaround_isdef_treekernel, index) mtree.segments[index] = Set(ido) @@ -495,14 +493,11 @@ function buildTree_Manellic!( r_PP, MVector{N,Float64}(weights), MVector{N,Int}(1:N), - lkern, # MVector{N,lknlT}(undef), + lkern, SizedVector{N,tknlT}(undef), - # SizedVector{N,tknlT}(undef), SizedVector{N,Set{Int}}(undef), - MVector{N,Int}(undef), - MVector{N,Int}(undef), + _workaround_isdef_leafkernel, Set{Int}(), - _workaround_isdef_leafkernel ) # @@ -517,15 +512,39 @@ function buildTree_Manellic!( # manual reset leaves in the order discovered permute!(tosort_leaves.leaf_kernels, tosort_leaves.permute) - # dupl = deepcopy(tosort_leaves.leaf_kernels) - # for (k,i) in enumerate(tosort_leaves.permute) - # tosort_leaves[i] = dupl.leaf_kernels[k] - # end return tosort_leaves end +function updateBandwidths( + mtr::ManellicTree{M,D,N,HL}, + bws +) where {M,D,N,HL} + # + _getBW(s::Float64,::Int) = [s;;] + _getBW(s::AbstractVector{<:Real},::Int) = s + _getBW(s::AbstractMatrix{<:Real},::Int) = s + _getBW(s::AbstractVector{<:AbstractArray},_i::Int) = s[_i] + + _leaf_kernels = SizedVector{N,HL}(undef) + for (i,lk) in enumerate(mtr.leaf_kernels) + _leaf_kernels[i] = updateKernelBW(lk,_getBW(bws,i)) + end + ManellicTree( + mtr.manifold, + mtr.data, + mtr.weights, + mtr.permute, + _leaf_kernels, + mtr.tree_kernels, + mtr.segments, + mtr._workaround_isdef_leafkernel, + mtr._workaround_isdef_treekernel, + ) +end + + """ $SIGNATURES @@ -582,9 +601,9 @@ function evaluate( pt, LOO::Bool = false, ) where {M,D,N,HL} - # force function barrier, just to be sure dyndispatch is limited - _F() = getfield(ApproxManifoldProducts,HL.name.name) - _F_ = _F() + # # force function barrier, just to be sure dyndispatch is limited + # _F() = getfield(ApproxManifoldProducts,HL.name.name) + # _F_ = _F() pts = getPoints(mt) w = getWeights(mt) diff --git a/src/services/ManifoldKernelDensity.jl b/src/services/ManifoldKernelDensity.jl index 05bb1b9..42d47dc 100644 --- a/src/services/ManifoldKernelDensity.jl +++ b/src/services/ManifoldKernelDensity.jl @@ -70,16 +70,18 @@ function ManifoldKernelDensity( arr[:,j] = vee(M, ϵ, log(M, ϵ, vecP[j])) end - manis = convert(Tuple, M) - # find or have the bandwidth - _bw = bw === nothing ? getKDEManifoldBandwidths(arr, manis ) : bw + # FIXME ON FIRE REMOVE LEGACY + manis = convert(Tuple, M) + # find or have the bandwidth + _bw = isnothing(bw) ? getKDEManifoldBandwidths(arr, manis ) : bw # NOTE workaround for partials and user did not specify a bw - if bw === nothing && partial !== nothing + if isnothing(bw) && !isnothing(partial) mask = ones(Int, length(_bw)) .== 1 mask[partial] .= false _bw[mask] .= 1.0 end - addopT, diffopT, _, _ = buildHybridManifoldCallbacks(manis) + # FIXME ON FIRE REMOVE LEGACY + addopT, diffopT, _, _ = buildHybridManifoldCallbacks(manis) bel = belmodel(arr,_bw,addopT,diffopT) # bel = KernelDensityEstimate.kde!(arr, collect(_bw), addopT, diffopT) return ManifoldKernelDensity(M, bel, partial, u0, infoPerCoord) @@ -101,6 +103,7 @@ manikde!( # + function manikde!_manellic( M::AbstractManifold, pts::AbstractVector; @@ -118,7 +121,7 @@ function manikde!_manellic( # Cost function to optimize _cost(_pts, σ) = begin # FIXME avoid rebuilding tree at each optim iteration!!! - mtr = buildTree_Manellic!(M, _pts; kernel_bw=reshape([σ;],manifold_dimension(M),1), kernel=MvNormalKernel) + mtr = buildTree_Manellic!(M, _pts; kernel_bw=reshape(σ,manifold_dimension(M),1), kernel=MvNormalKernel) entropy(mtr) end @@ -127,27 +130,22 @@ function manikde!_manellic( # set lower and upper bounds for Golden section optimization lcov, ucov = getBandwidthSearchBounds(mtree) res = Optim.optimize( - (s)->_cost(pts,s^2), + (s)->_cost(pts,[s^2;]), lcov[1], ucov[1], Optim.GoldenSection() ) - best_cov = [Optim.minimizer(res);;] - + best_cov = [Optim.minimizer(res);] + + # reuse (heavy lift parts of) earlier tree build # return tree with correct bandwidth - # TODO avoid tree rebuild somehow manikde!( M, pts; - bw=best_cov, - belmodel = (a,b,aF,dF) -> ApproxManifoldProducts.buildTree_Manellic!( - M, - pts; - kernel_bw=b, - kernel=AMP.MvNormalKernel - ) + belmodel = (ignore...) -> updateBandwidths(mtree, best_cov) ) end + ## ========================================================================================== ## a few utilities ## ========================================================================================== @@ -305,7 +303,7 @@ function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P} try # mn = mean(mkd.manifold, getPoints(mkd, false)) mn = mean(mkd) - if mn isa ProductRepr + if mn isa ProductRepr # TODO UPDATE to ArrayPartition only, discontinued use of ProductRepr long ago. println(io) for prt in mn.parts println(io, " ", round.(prt,digits=4)) From c441bc11b5497324f285124b1cf4ab143355f076 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Mon, 13 May 2024 08:16:58 +0200 Subject: [PATCH 11/31] More testing on tree eval and products (#290) * More Testing on tree eval WIP (#289) * More testing on tree eval * products * Merge branch '24Q2/twig/kerevaltest' into 24Q2/test/test_mtree_eval2 * cleanup test * comment plotting --------- Co-authored-by: Johannes Terblanche --- src/services/KernelEval.jl | 14 +- test/manellic/testManellicTree.jl | 314 +++++++++++++++++++++++++++++- 2 files changed, 320 insertions(+), 8 deletions(-) mode change 100644 => 100755 test/manellic/testManellicTree.jl diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index c65145b..eaecc3e 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -4,11 +4,12 @@ function projectSymPosDef(c::AbstractMatrix) s = size(c) # pretty fast to make or remake isbitstype form matrix _c = SMatrix{s...}(c) + #TODO likely not intended project here: see AMP#283 issymmetric(_c) ? _c : project(SymmetricPositiveDefinite(s[1]),_c,_c) end function MvNormalKernel( - μ::AbstractVector, + μ::AbstractArray, Σ::AbstractArray, weight::Real=1.0 ) @@ -58,7 +59,7 @@ function Base.show(io::IO, mvk::MvNormalKernel) μ = mean(mvk) Σ2 = cov(mvk) # Σ=sqrt(Σ2) - d = length(μ) + d = size(Σ2,1) print(io, "MvNormalKernel(d=",d) print(io,",μ=",round.(μ;digits=3)) print(io,",Σ^2=[",round(Σ2[1];digits=3)) @@ -97,10 +98,11 @@ function distanceMalahanobisSq( basis=DefaultOrthogonalBasis() ) δc = distanceMalahanobisCoordinates(M,K,q,basis) - p = mean(K) - ϵ = identity_element(M, q) - X = get_vector(M, ϵ, δc, basis) - return inner(M, p, X, X) + # p = mean(K) + # ϵ = identity_element(M, q) + # X = get_vector(M, ϵ, δc, basis) + # return inner(M, p, X, X) + return δc'*δc end diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl old mode 100644 new mode 100755 index 8a50e88..bcc2328 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -3,6 +3,7 @@ using Test using ApproxManifoldProducts using Random +using LinearAlgebra using StaticArrays using TensorCast using Manifolds @@ -255,7 +256,6 @@ permref = sortperm(pts, by=s->getindex(s,1)) @test norm((pts[mtree.permute] .- mean.(mtree.leaf_kernels)) .|> s->s[1]) < 1e-6 - # for (i,v) in enumerate(dict[:evaltest_1_at]) # # @show AMP.evaluate(mtree, [v;]), dict[:evaltest_1_dens][i] # @test isapprox(dict[:evaltest_1_dens][i], AMP.evaluate(mtree, [v;])) @@ -266,6 +266,316 @@ permref = sortperm(pts, by=s->getindex(s,1)) ## end +@testset "Test evaluate MvNormalKernel" begin + +M = TranslationGroup(1) +ker = AMP.MvNormalKernel([0.0], [0.5;;]) +@test isapprox( + AMP.evaluate(M, ker, [0.1]), + pdf(MvNormal(mean(ker), cov(ker)), [0.1]) +) + + +# Test wrapped cicular distribution +function pdf_wrapped_normal(μ, σ, θ; nwrap=1000) + s = 0.0 + for k = -nwrap:nwrap + s += exp(-(θ - μ + 2pi*k)^2 / (2*σ^2)) + end + return 1/(σ*sqrt(2pi)) * s +end + +M = RealCircleGroup() +ker = AMP.MvNormalKernel([0.0], [0.1;;]) +@test isapprox( + AMP.evaluate(M, ker, [0.1]), + pdf_wrapped_normal(mean(ker)[], sqrt(cov(ker))[], 0.1) +) + +ker = AMP.MvNormalKernel([0], [2.0;;]) +@test isapprox( + AMP.evaluate(M, ker, [0.]), + AMP.evaluate(M, ker, [2pi]) +) +#TODO wrapped normal distributions broken +@test_broken isapprox( + pdf_wrapped_normal(mean(ker)[], sqrt(cov(ker))[], pi), + AMP.evaluate(M, ker, [pi]) +) +@test_broken isapprox( + pdf_wrapped_normal(mean(ker)[], sqrt(cov(ker))[], 0), + AMP.evaluate(M, ker, [0.]) +) + +## +M = SpecialEuclidean(2) +ϵ = identity_element(M) +Xc = [10, 20, 0.1] +p = exp(M, ϵ, hat(M, ϵ, Xc)) +kercov = diagm([0.5, 2.0, 0.1].^2) +ker = AMP.MvNormalKernel(p, kercov) +@test isapprox( + AMP.evaluate(M, ker, p), + pdf(MvNormal(Xc, cov(ker)), Xc) +) + +Xc = [10, 22, -0.1] +q = exp(M, ϵ, hat(M, ϵ, Xc)) + +@test isapprox( + pdf(MvNormal(cov(ker)), [0,0,0]), + AMP.evaluate(M, ker, p) +) + +X = log(M, ϵ, Manifolds.compose(M, inv(M, p), q)) +Xc_e = vee(M, ϵ, X) +pdf_local_coords = pdf(MvNormal(cov(ker)), Xc_e) + +@test isapprox( + pdf_local_coords, + AMP.evaluate(M, ker, q), +) + +delta_c = AMP.distanceMalahanobisCoordinates(M, ker, q) +X = log(M, ϵ, Manifolds.compose(M, inv(M, p), q)) +Xc_e = vee(M, ϵ, X) +malad_t = Xc_e'*inv(kercov)*Xc_e +# delta_t = [10, 20, 0.1] - [10, 22, -0.1] +@test isapprox( + malad_t, + delta_c'*delta_c; + atol=1e-10 +) + +malad2 = AMP.distanceMalahanobisSq(M,ker,q) +@test isapprox( + malad_t, + malad2; + atol=1e-10 +) + +rbfd = AMP.ker(M, ker, q, 0.5, AMP.distanceMalahanobisSq) +@test isapprox( + exp(-0.5*malad_t), + rbfd; + atol=1e-10 +) + + +# NOTE 'global' distribution would have been +X = log(M, mean(ker), q) +Xc_e = vee(M, ϵ, X) +pdf_global_coords = pdf(MvNormal(cov(ker)), Xc_e) + + +end + +@testset "Basic ManellicTree manifolds construction and evaluations" begin +## + + +M = TranslationGroup(1) +ϵ = identity_element(M) +dis = MvNormal([3.0], diagm([1.0].^2)) +Cpts = [rand(dis) for _ in 1:128] +pts = map(c->exp(M, ϵ, hat(M, ϵ, c)), Cpts) +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = [0.2;;], kernel=AMP.MvNormalKernel) + +## +p = exp(M, ϵ, hat(M, ϵ, [3.0])) +y_amp = AMP.evaluate(mtree, p) + +y_pdf = pdf(dis, [3.0]) + +@test isapprox(y_amp, y_pdf; atol=0.1) + +# ps = [[p] for p = -0:0.01:6] +# ys_amp = map(p->AMP.evaluate(mtree, exp(M, ϵ, hat(M, ϵ, p))), ps) +# ys_pdf = pdf(dis, ps) + +# lines(first.(ps), ys_pdf) +# lines!(first.(ps), ys_amp) + +# lines!(first.(ps), ys_pdf) +# lines(first.(ps), ys_amp) +## + +M = SpecialOrthogonal(2) +ϵ = identity_element(M) +dis = MvNormal([0.0], diagm([0.1].^2)) +Cpts = [rand(dis) for _ in 1:128] +pts = map(c->exp(M, ϵ, hat(M, ϵ, c)), Cpts) +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = [0.005;;], kernel=AMP.MvNormalKernel) + +## +p = exp(M, ϵ, hat(M, ϵ, [0.1])) +y_amp = AMP.evaluate(mtree, p) + +y_pdf = pdf(dis, [0.1]) + +@test isapprox(y_amp, y_pdf; atol=0.5) + +ps = [[p] for p = -0.3:0.01:0.3] +ys_amp = map(p->AMP.evaluate(mtree, exp(M, ϵ, hat(M, ϵ, p))), ps) +ys_pdf = pdf(dis, ps) + +# lines(first.(ps), ys_pdf) +# lines!(first.(ps), ys_amp) + + +M = SpecialEuclidean(2) +ϵ = identity_element(M) +dis = MvNormal([10,20,0.1], diagm([0.5,2.0,0.1].^2)) +Cpts = [rand(dis) for _ in 1:128] +pts = map(c->exp(M, ϵ, hat(M, ϵ, c)), Cpts) +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = diagm([0.05,0.2,0.01]), kernel=AMP.MvNormalKernel) + +## +p = exp(M, ϵ, hat(M, ϵ, [10, 20, 0.1])) +y_amp = AMP.evaluate(mtree, p) +y_pdf = pdf(dis, [10,20,0.1]) +#FIXME +@test_broken isapprox(y_amp, y_pdf; atol=0.1) + +end + + +## ======================================================================================== +@testset "Test Product the brute force way" begin + +M = SpecialEuclidean(2) +ϵ = identity_element(M) + +Xc_p = [10, 20, 0.1] +p = exp(M, ϵ, hat(M, ϵ, Xc_p)) +kerp = AMP.MvNormalKernel(p, diagm([0.5, 2.0, 0.1].^2)) + +Xc_q = [10, 22, -0.1] +q = exp(M, ϵ, hat(M, ϵ, Xc_q)) +kerq = AMP.MvNormalKernel(q, diagm([1.0, 1.0, 0.1].^2)) + +kerpq = calcProductGaussians(M, [kerp, kerq]) + +# brute force way +xs = 7:0.1:13 +ys = 15:0.1:27 +θs = -0.3:0.01:0.3 + +grid_points = map(Iterators.product(xs, ys, θs)) do (x,y,θ) + exp(M, ϵ, hat(M, ϵ, SVector(x,y,θ))) +end + +# use_global_coords = true +use_global_coords = false + +pdf_ps = map(grid_points) do gp + if use_global_coords + X = log(M, p, gp) + Xc_e = vee(M, ϵ, X) + pdf(MvNormal(cov(kerp)), Xc_e) + else + X = log(M, ϵ, Manifolds.compose(M, inv(M, p), gp)) + Xc_e = vee(M, ϵ, X) + pdf(MvNormal(cov(kerp)), Xc_e) + end +end + +pdf_qs = map(grid_points) do gp + if use_global_coords + X = log(M, q, gp) + Xc_e = vee(M, ϵ, X) + pdf(MvNormal(cov(kerq)), Xc_e) + else + X = log(M, ϵ, Manifolds.compose(M, inv(M, q), gp)) + Xc_e = vee(M, ϵ, X) + pdf(MvNormal(cov(kerq)), Xc_e) + end +end + +pdf_pqs = pdf_ps .* pdf_qs +# pdf_pqs ./= sum(pdf_pqs) * 0.01 +# pdf_pqs .*= 15.9672 + +amp_ps = map(grid_points) do gp + AMP.evaluate(M, kerp, gp) +end +amp_qs = map(grid_points) do gp + AMP.evaluate(M, kerq, gp) +end + +amp_pqs = map(grid_points) do gp + AMP.evaluate(M, kerpq, gp) +end + +amp_bf_pqs = amp_ps .* amp_qs + +#FIXME +normalized_compare_test = isapprox.(normalize(amp_pqs), normalize(amp_bf_pqs); atol=0.001) +@test_broken all(normalized_compare_test) +@warn "Brute force product test overlap $(round(count(normalized_compare_test) / length(amp_pqs) * 100, digits=2))%" + +#TODO should this be local or global coords? +@test_broken findmax(pdf_pqs[:,60,30])[2] == findmax(amp_pqs[:,60,30])[2] + +# these are all correct +# lines(xs, pdf_ps[:,60,30]) +# lines!(xs, amp_ps[:,60,30]) + +# lines(ys, pdf_ps[30,:,30]) +# lines!(ys, amp_ps[30,:,30]) + +# lines(θs, pdf_ps[30,60,:]) +# lines!(θs, amp_ps[30,60,:]) + +#these are different for "local" vs "global" +# lines(xs, normalize(pdf_pqs[:,60,30])) +# lines!(xs, normalize(amp_pqs[:,60,30])) +# lines!(xs, normalize(amp_bf_pqs[:,60,30])) + +# lines(ys, normalize(pdf_pqs[30,:,30])) +# lines!(ys, normalize(amp_pqs[30,:,30])) + +# lines(θs, normalize(pdf_pqs[30,60,:])) +# lines!(θs, normalize(amp_pqs[30,60,:])) + +# contour(xs, ys, pdf_pqs[:,:,30]; color = :blue) +# contour!(xs, ys, amp_pqs[:,:,30]; color = :red) +# contour!(xs, ys, amp_bf_pqs[:,:,30]; color = :green) + +#just some exploration +# pdf_p = pdf(Normal(10, 0.5), xs) +# pdf_q = pdf(Normal(10, 1.0), xs) +# pdf_pq = (pdf_p .* pdf_q) +# pdf_pq ./= sum(pdf_pq) * 0.01 + +# lines(xs, pdf_p) +# lines!(xs, pdf_q) +# lines!(xs, pdf_pq) + +# pdf_p = pdf(Normal(20, 2.0), ys) +# pdf_q = pdf(Normal(22, 1.0), ys) +# pdf_pq = (pdf_p .* pdf_q) +# pdf_pq ./= sum(pdf_pq) * 0.01 + +# lines(ys, pdf_p) +# lines!(ys, pdf_q) +# lines!(ys, pdf_pq) + +# pdf_p = pdf(Normal(0.1, 0.1), θs) +# pdf_q = pdf(Normal(-0.1, 0.1), θs) +# pdf_pq = (pdf_p .* pdf_q) +# pdf_pq ./= sum(pdf_pq) * 0.01 + +# lines(θs, pdf_p) +# lines!(θs, pdf_q) +# lines!(θs, pdf_pq) + + +end + +## ======================================================================================== + + @testset "Manellic basic evaluation test 1D" begin ## @@ -274,7 +584,7 @@ pts = [zeros(1) for _ in 1:100] bw = ones(1,1) mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw, kernel=AMP.MvNormalKernel) -@test isapprox( 0.4, AMP.evaluate(mtree, SA[0.0;]); atol=0.1) +@test isapprox( pdf(Normal(0,1), 0), AMP.evaluate(mtree, SA[0.0;])) @error "expectedLogL for different number of test points not working yet." # AMP.expectedLogL(mtree, [randn(1) for _ in 1:5]) From 519da8cbec46ae31c4da5f468ad460036311b6e3 Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Mon, 13 May 2024 05:07:16 -0400 Subject: [PATCH 12/31] manellic construction uses updateBW instead (#291) --- src/entities/KernelEval.jl | 2 +- src/services/KernelEval.jl | 20 +++++--- src/services/ManellicTree.jl | 16 +++++-- src/services/ManifoldKernelDensity.jl | 8 ++-- test/manellic/testManellicTree.jl | 68 +++++++++++++++++++++++++-- 5 files changed, 94 insertions(+), 20 deletions(-) diff --git a/src/entities/KernelEval.jl b/src/entities/KernelEval.jl index d9587c9..db9e9fe 100644 --- a/src/entities/KernelEval.jl +++ b/src/entities/KernelEval.jl @@ -9,7 +9,7 @@ abstract type AbstractKernel end p::MvNormal{T,M} # TDB might already be covered in p.Σ.chol but having issues with SymPD (not particular to this AMP repo) """ Manually maintained square root concentration matrix for faster compute, TODO likely duplicate of existing Distrubtions.jl functionality. """ - sqrt_iΣ::iM = sqrt(inv(p.Σ)) + sqrt_iΣ::iM = sqrt(inv(cov(p))) """ Nonparametric weight value """ weight::Float64 = 1.0 end diff --git a/src/services/KernelEval.jl b/src/services/KernelEval.jl index eaecc3e..69ffdc2 100644 --- a/src/services/KernelEval.jl +++ b/src/services/KernelEval.jl @@ -24,12 +24,22 @@ Statistics.mean(m::MvNormalKernel) = m.μ # mean(m.p) # m.p.μ Statistics.cov(m::MvNormalKernel) = cov(m.p) # note also about m.sqrt_iΣ Statistics.std(m::MvNormalKernel) = sqrt(cov(m)) -updateKernelBW(k::MvNormalKernel,_bw) = (p=MvNormal(_bw); MvNormalKernel(;μ=k.μ,p,weight=k.weight)) +function updateKernelBW( + k::MvNormalKernel, + _bw, + isq_bw = inv(sqrt(_bw)) +) + p=MvNormal(_bw) + sqrt_iΣ = typeof(k.sqrt_iΣ)(isq_bw) + return MvNormalKernel(;μ=k.μ,p,sqrt_iΣ,weight=k.weight) +end +updateKernelBW(ekr::MvNormalKernel, ::Nothing) = ekr # avoid ifs for noops + function evaluate( M::AbstractManifold, ekr::MvNormalKernel, - p # on manifold point + p, # on manifold point ) # dim = manifold_dimension(M) @@ -98,10 +108,7 @@ function distanceMalahanobisSq( basis=DefaultOrthogonalBasis() ) δc = distanceMalahanobisCoordinates(M,K,q,basis) - # p = mean(K) - # ϵ = identity_element(M, q) - # X = get_vector(M, ϵ, δc, basis) - # return inner(M, p, X, X) + # return inner(M, p, X, X) # did not work as inner gave almost 2x the answer? return δc'*δc end @@ -114,7 +121,6 @@ function _distance( p=MvNormal(_p,SVector(ntuple((s)->1,manifold_dimension(M))...)) ), distFnc::Function=distanceMalahanobisSq, - # distFnc::Function=distanceMalahanobisSq, ) distFnc(M, kernel(p), q) end diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index b54245a..528dd4f 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -199,7 +199,7 @@ function Base.show( printstyled(io,"::TK ";color=:magenta) println(io) end - printstyled(io, " (depth) : ", floor(Int,log2(length(mt.tree_kernels))),"+1"; color=:light_black) + printstyled(io, " (depth) : 1+", floor(Int,log2(length(mt.tree_kernels))); color=:light_black) println(io) printstyled(io, " (blncd) : ", "true : _wip_";color=:light_black) println(io) @@ -600,6 +600,7 @@ function evaluate( mt::ManellicTree{M,D,N,HL}, pt, LOO::Bool = false, + force_kbw = nothing ) where {M,D,N,HL} # # force function barrier, just to be sure dyndispatch is limited # _F() = getfield(ApproxManifoldProducts,HL.name.name) @@ -616,8 +617,10 @@ function evaluate( # FIXME, is this assuming length(pts) and length(mt.leaf_kernels) are the same? # FIXME use consolidated getKernelLeaf instead ekr = mt.leaf_kernels[i] + ekr = updateKernelBW(ekr, force_kbw) # TODO remember special handling for partials in the future oneval = mt.weights[i] * evaluate(mt.manifold, ekr, pt) + # leave one out requires kernel weighting to removal of leave out weight oneval *= !LOO ? 1 : 1/(1-w[i]) sumval += oneval end @@ -645,6 +648,7 @@ function evaluateDensityAtPoints( ) # evaluate new sampling weights of points in out component + # TODO use agnostic-Dual tree or MonteCarloDualTree evaluation # vector for storing resulting weights smw = zeros(length(eval_at_points)) for (i,ev) in enumerate(eval_at_points) @@ -666,14 +670,15 @@ end function expectedLogL( mt::ManellicTree{M,D,N}, epts::AbstractVector, - LOO::Bool = false + LOO::Bool = false, + force_kbw = nothing ) where {M,D,N} T = Float64 - # TODO really slow brute force evaluation + # TODO really slow brute force evaluation, use agnostic-DualTree or MonteCarloDualTree eL = MVector{length(epts),T}(undef) for (i,p) in enumerate(epts) # LOO skip for leave-one-out - eL[i] = evaluate(mt, p, LOO) + eL[i] = evaluate(mt, p, LOO, force_kbw) end # set numerical tolerance floor zrs = findall(isapprox.(0,eL)) @@ -693,7 +698,8 @@ end entropy( mt::ManellicTree, -) = -expectedLogL(mt, getPoints(mt), true) + force_kbw = nothing +) = -expectedLogL(mt, getPoints(mt), true, force_kbw) (mt::ManellicTree)( diff --git a/src/services/ManifoldKernelDensity.jl b/src/services/ManifoldKernelDensity.jl index 42d47dc..998f7c2 100644 --- a/src/services/ManifoldKernelDensity.jl +++ b/src/services/ManifoldKernelDensity.jl @@ -120,9 +120,9 @@ function manikde!_manellic( # Cost function to optimize _cost(_pts, σ) = begin - # FIXME avoid rebuilding tree at each optim iteration!!! - mtr = buildTree_Manellic!(M, _pts; kernel_bw=reshape(σ,manifold_dimension(M),1), kernel=MvNormalKernel) - entropy(mtr) + # avoid rebuilding tree at each optim iteration!!! + # mtr = buildTree_Manellic!(M, _pts; kernel_bw=reshape(σ,manifold_dimension(M),1), kernel=MvNormalKernel) + entropy(mtree,reshape(σ,manifold_dimension(M),1)) end # optimize for best LOOCV bandwidth @@ -133,7 +133,7 @@ function manikde!_manellic( (s)->_cost(pts,[s^2;]), lcov[1], ucov[1], Optim.GoldenSection() ) - best_cov = [Optim.minimizer(res);] + best_cov = [Optim.minimizer(res);;] # reuse (heavy lift parts of) earlier tree build # return tree with correct bandwidth diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index bcc2328..93d3506 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -629,6 +629,22 @@ AMP.expectedLogL(mtree, pts) @test AMP.expectedLogL(mtree, pts) < Inf +# to enable faster bandwidth selection/optimization +ekr = ApproxManifoldProducts.getKernelLeaf(mtree,1,false) +ekr_ = ApproxManifoldProducts.updateKernelBW(ekr,SA[1.0;;]) + +@test typeof(ekr) == typeof(ekr_) + +# confirm that updating the bandwidths works properly +Σ = [0.1+0.5*rand();;] + +mtr = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=Σ,kernel=AMP.MvNormalKernel) +mtr_ = ApproxManifoldProducts.updateBandwidths(mtree, Σ) + +# +@test isapprox( mtr([0.0]), mtr_([0.0]); atol=1e-10) +@test isapprox( ApproxManifoldProducts.entropy(mtr), ApproxManifoldProducts.entropy(mtr_); atol=1e-10) + ## end @@ -711,8 +727,7 @@ Y = S .|> s->cost(pts,s^2) # should pass the optimal kbw somewhere in the given range @test any(0 .< diff(Y)) -# and optimize - +# and optimize with rebuild tree cost res = Optim.optimize( (s)->cost(pts,s^2), 0.05, 3.0, Optim.GoldenSection() @@ -720,6 +735,38 @@ res = Optim.optimize( best_cov = Optim.minimizer(res) @test isapprox(0.5, best_cov; atol=0.3) +bcov_ = deepcopy(best_cov) + +## Test more efficient updateKernelBW version + +cost2(σ) = begin + mtr = ApproxManifoldProducts.updateBandwidths(mtree_0, [σ;;]) + AMP.entropy(mtr) +end + +# and optimize with "update" kernel bandwith cost +res = Optim.optimize( + (s)->cost2(s^2), + 0.05, 3.0, Optim.GoldenSection() +) +@show best_cov = Optim.minimizer(res) + +@test isapprox(bcov_, best_cov; atol=1e-3) + +# mask bandwith by passing in an alternative + +cost3(σ) = begin + AMP.entropy(mtree_0, [σ;;]) +end + +# and optimize with "update" kernel bandwith cost +res = Optim.optimize( + (s)->cost3(s^2), + 0.05, 3.0, Optim.GoldenSection() +) +@show best_cov = Optim.minimizer(res) + +@test isapprox(bcov_, best_cov; atol=1e-3) ## @@ -734,10 +781,25 @@ end M = TranslationGroup(1) # pts = [[0.;],[0.1],[0.2;],[0.3;]] -pts = [1*randn(1) for _ in 1:64] +pts = [1*randn(1) for _ in 1:128] mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) +best_cov = cov(ApproxManifoldProducts.getKernelLeaf(mkd.belief,1))[1] |> sqrt +@show best_cov + +@test isapprox(0.5, best_cov; atol=0.3) + +# remember broken code in get w bounds + +try + pts = [1*randn(1) for _ in 1:100] + mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) +catch + @test_broken false +end + + ## end From 8a0e6fdb591c7f259a694712af2da3226c139b0c Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Sun, 19 May 2024 17:11:44 -0400 Subject: [PATCH 13/31] multidim optim of manellic and test (#292) * multidim optim of manellic and test * all content * manikde!_manellic for TranslationGroup(2) * fix testManellic univariate --- src/services/ManifoldKernelDensity.jl | 32 +++++++++++++-------- test/manellic/testManellicTree.jl | 40 ++++++++++++++++++++++++++- 2 files changed, 59 insertions(+), 13 deletions(-) diff --git a/src/services/ManifoldKernelDensity.jl b/src/services/ManifoldKernelDensity.jl index 998f7c2..49d0010 100644 --- a/src/services/ManifoldKernelDensity.jl +++ b/src/services/ManifoldKernelDensity.jl @@ -107,7 +107,7 @@ manikde!( function manikde!_manellic( M::AbstractManifold, pts::AbstractVector; - bw=ones(manifold_dimension(M),1), + bw=diagm(ones(manifold_dimension(M))), ) # @@ -119,21 +119,29 @@ function manikde!_manellic( ) # Cost function to optimize - _cost(_pts, σ) = begin - # avoid rebuilding tree at each optim iteration!!! - # mtr = buildTree_Manellic!(M, _pts; kernel_bw=reshape(σ,manifold_dimension(M),1), kernel=MvNormalKernel) - entropy(mtree,reshape(σ,manifold_dimension(M),1)) - end + # avoid rebuilding tree at each optim iteration!!! + _cost(σ::Real) = entropy(mtree,[σ^2;;]) # reshape(σ,manifold_dimension(M),1)) + _cost(σ::AbstractVector) = entropy(mtree,diagm(σ.^2)) # reshape(σ,manifold_dimension(M),1)) + _cost(σ::AbstractMatrix) = entropy(mtree,σ.^2) # reshape(σ,manifold_dimension(M),1)) # optimize for best LOOCV bandwidth # FIXME switch to RLM (or other Manopt) techinque instead # set lower and upper bounds for Golden section optimization - lcov, ucov = getBandwidthSearchBounds(mtree) - res = Optim.optimize( - (s)->_cost(pts,[s^2;]), - lcov[1], ucov[1], Optim.GoldenSection() - ) - best_cov = [Optim.minimizer(res);;] + best_cov = if 1 === manifold_dimension(M) + lcov, ucov = getBandwidthSearchBounds(mtree) + res = Optim.optimize( + (s)->_cost([s;]), + lcov[1], ucov[1], Optim.GoldenSection() + ) + [Optim.minimizer(res);;] + else + res = Optim.optimize( + _cost, + diag(bw), # FIXME Optim API issue, if using bw::matrix then steps not PDMat (NelderMead) + Optim.NelderMead() + ) + diagm(Optim.minimizer(res)) + end # reuse (heavy lift parts of) earlier tree build # return tree with correct bandwidth diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 93d3506..9c9c1e7 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -732,6 +732,7 @@ res = Optim.optimize( (s)->cost(pts,s^2), 0.05, 3.0, Optim.GoldenSection() ) + best_cov = Optim.minimizer(res) @test isapprox(0.5, best_cov; atol=0.3) @@ -749,6 +750,7 @@ res = Optim.optimize( (s)->cost2(s^2), 0.05, 3.0, Optim.GoldenSection() ) + @show best_cov = Optim.minimizer(res) @test isapprox(bcov_, best_cov; atol=1e-3) @@ -764,7 +766,8 @@ res = Optim.optimize( (s)->cost3(s^2), 0.05, 3.0, Optim.GoldenSection() ) -@show best_cov = Optim.minimizer(res) + +best_cov = Optim.minimizer(res) @test isapprox(bcov_, best_cov; atol=1e-3) @@ -804,6 +807,41 @@ end end +@testset "Multidimensional LOOCV bandwidth optimization" begin +## + +M = TranslationGroup(2) +pts = [1*randn(2) for _ in 1:64] + +bw = [1.0; 1.0] +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) + +cost4(σ) = begin + AMP.entropy(mtree, diagm(σ.^2)) +end + +# and optimize with "update" kernel bandwith cost +@time res = Optim.optimize( + cost4, + bw, + Optim.NelderMead() +); + +@test res.ls_success + +@show best_cov = Optim.minimizer(res) + +@test isapprox([0.5; 0.5], best_cov; atol=0.3) + + +mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) + +@test isapprox([0.5 0; 0 0.5], getBW(mkd)[1]; atol=0.3) + + +## +end + ## # # using GLMakie From 20c066ae96c494d4049d6b6264583dfa26994f72 Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Sun, 19 May 2024 23:52:37 -0400 Subject: [PATCH 14/31] multidim manellic on SE2 (#293) * mulitdim manellic on SE2 * manellic optim abs best_cov for SPD * bump test manellic optim checks * rm excess test printouts * test manellic bump criteria * test manellic better printout spot check --- src/services/ManellicTree.jl | 2 +- src/services/ManifoldKernelDensity.jl | 5 +-- test/manellic/testManellicTree.jl | 47 ++++++++++++++++++++++++--- test/testMarginalProducts.jl | 20 ++++++------ 4 files changed, 57 insertions(+), 17 deletions(-) diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index 528dd4f..aa2ba8e 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -248,7 +248,7 @@ function Base.convert( ) where {P,T,M,iM} # _matType(::Type{Distributions.PDMats.PDMat{_F,_M}}) where {_F,_M} = _M - μ = P(src.μ) + μ = convert(P,src.μ) # P(src.μ) p = MvNormal(_matType(M)(cov(src.p))) sqrt_iΣ = iM(src.sqrt_iΣ) MvNormalKernel{P,T,M,iM}(μ, p, sqrt_iΣ, src.weight) diff --git a/src/services/ManifoldKernelDensity.jl b/src/services/ManifoldKernelDensity.jl index 49d0010..e692be6 100644 --- a/src/services/ManifoldKernelDensity.jl +++ b/src/services/ManifoldKernelDensity.jl @@ -108,6 +108,7 @@ function manikde!_manellic( M::AbstractManifold, pts::AbstractVector; bw=diagm(ones(manifold_dimension(M))), + algo = Optim.NelderMead() ) # @@ -138,9 +139,9 @@ function manikde!_manellic( res = Optim.optimize( _cost, diag(bw), # FIXME Optim API issue, if using bw::matrix then steps not PDMat (NelderMead) - Optim.NelderMead() + algo ) - diagm(Optim.minimizer(res)) + diagm(abs.(Optim.minimizer(res))) end # reuse (heavy lift parts of) earlier tree build diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 9c9c1e7..175f54d 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -7,6 +7,7 @@ using LinearAlgebra using StaticArrays using TensorCast using Manifolds +import Rotations as Rot_ using Distributions import ApproxManifoldProducts: ManellicTree, eigenCoords, splitPointsEigen @@ -36,8 +37,8 @@ function testEigenCoords( # spot check @show _ax_ERR = log_lie(SpecialOrthogonal(2), (r_R_ax_')*r_R_ax)[1,2] - @show testval = isapprox(0, _ax_ERR; atol = 5/length(ax_CC)) - @assert testval "Spot check failed on eigen split of manifold points, the estimated point rotation matrix did not match construction." + @show testval = isapprox(0, _ax_ERR; atol = 6/length(ax_CC)) + @assert testval "Spot check failed on eigen split of manifold points, the estimated point rotation matrix did not match construction. length(ax_CC)=$(length(ax_CC))" r_CC, r_R_ax_, pidx, r_CV end @@ -807,7 +808,7 @@ end end -@testset "Multidimensional LOOCV bandwidth optimization" begin +@testset "Multidimensional LOOCV bandwidth optimization, TranslationGroup(2)" begin ## M = TranslationGroup(2) @@ -829,7 +830,7 @@ end @test res.ls_success -@show best_cov = Optim.minimizer(res) +@show best_cov = abs.(Optim.minimizer(res)) @test isapprox([0.5; 0.5], best_cov; atol=0.3) @@ -843,6 +844,44 @@ mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) end + +@testset "Multidimensional LOOCV bandwidth optimization, SpecialEuclidean(2)" begin +## + +M = SpecialEuclidean(2) +pts = [ArrayPartition(randn(2),Rot_.RotMatrix{2}(0.1*randn()).mat) for _ in 1:64] + +bw = [1.0; 1.0; 0.3] +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) + +cost4(σ) = begin + AMP.entropy(mtree, diagm(σ.^2)) +end + +# and optimize with "update" kernel bandwith cost +@time res = Optim.optimize( + cost4, + bw, + Optim.NelderMead() +); + +@test res.ls_success + +@show best_cov = abs.(Optim.minimizer(res)) + +@test isapprox([0.6; 0.6; 0.06], best_cov; atol=0.35) + + +mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) + +@test isapprox([0.6 0 0; 0 0.6 0; 0 0 0.06], getBW(mkd)[1]; atol=0.35) + + +## +end + + + ## # # using GLMakie diff --git a/test/testMarginalProducts.jl b/test/testMarginalProducts.jl index 9cddfbe..0a42dcf 100644 --- a/test/testMarginalProducts.jl +++ b/test/testMarginalProducts.jl @@ -47,8 +47,8 @@ P12 = manifoldProduct([P1;P2], recordLabels=true, selectedLabels=sl, addEntropy= # @test isapprox( mean(P12)[1], 0, atol=1 ) # @test isapprox( mean(P12)[2], 0, atol=1 ) -(x->println()).(1:5) -@show sl; + +# @show sl; P12 @@ -99,7 +99,7 @@ P12_ = manifoldProduct([P1;P2_], recordLabels=true, selectedLabels=sl, addEntrop @test isapprox( mean(P12_)[1], 0, atol=1 ) @test isapprox( mean(P12_)[2], 0, atol=1 ) -(x->println()).(1:5) + # @show sl P12_ @@ -156,7 +156,7 @@ P123_ = manifoldProduct([P1;P2_;P3_], recordLabels=true, selectedLabels=sl, addE @test isapprox( mean(P123_)[1], 0, atol=1 ) @test isapprox( mean(P123_)[2], 0, atol=1 ) -(x->println()).(1:5) + # @show sl P123_ @@ -207,7 +207,7 @@ P = manifoldProduct([P2;P1;P3], recordLabels=true, selectedLabels=sl, addEntropy @test !isPartial(P) -(x->println()).(1:5) + # @show sl; P @@ -267,7 +267,7 @@ P_ = manifoldProduct([P1;P3], recordLabels=true, selectedLabels=sl, addEntropy=f @test !isPartial(P_) -(x->println()).(1:5) + # @show sl ## @@ -329,7 +329,7 @@ P45__ = manifoldProduct([P4;P4_;P5;P5_], recordLabels=true, selectedLabels=sl, a @test !isPartial(P45__) -(x->println()).(1:5) + # @show sl; P45__ @@ -393,7 +393,7 @@ P = manifoldProduct([P2;P1;P3], recordLabels=true, selectedLabels=sl, addEntropy @test !isPartial(P) -(x->println()).(1:5) + # @show sl; P @@ -456,7 +456,7 @@ P = manifoldProduct([P1;P2;P3], recordLabels=true, selectedLabels=sl, addEntropy @test !isPartial(P) -(x->println()).(1:5) + # @show sl; P @@ -517,7 +517,7 @@ P = manifoldProduct([P1;P3], recordLabels=true, selectedLabels=sl, addEntropy=fa @test isPartial(P) @test P._partial == [1;3] -(x->println()).(1:5) + # @show sl; P From 357b989185001aa66a482463b93ca4a3a464cbde Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Mon, 20 May 2024 00:26:30 -0400 Subject: [PATCH 15/31] test manellic construct SE3 (#294) * test manellic construct SE3 * bug fix --- test/manellic/testManellicTree.jl | 44 ++++++++++++++++++++++++++++--- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 175f54d..3675ae2 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -869,12 +869,14 @@ end @show best_cov = abs.(Optim.minimizer(res)) -@test isapprox([0.6; 0.6; 0.06], best_cov; atol=0.35) +@test isapprox([0.6; 0.6], best_cov[1:2]; atol=0.35) +@test isapprox(0.06, best_cov[3]; atol=0.04) mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) -@test isapprox([0.6 0 0; 0 0.6 0; 0 0 0.06], getBW(mkd)[1]; atol=0.35) +@test isapprox([0.6 0; 0 0.6], getBW(mkd)[1][1:2,1:2]; atol=0.35) +@test isapprox(0.06, getBW(mkd)[1][3,3]; atol=0.04) ## @@ -882,6 +884,42 @@ end +@testset "Multidimensional LOOCV bandwidth optimization, SpecialEuclidean(3)" begin +## + +M = SpecialEuclidean(3) +pts = [ArrayPartition(SA[randn(3)...;],SMatrix{3,3,Float64}(collect(Rot_.RotXYZ(0.1*randn(3)...)))) for _ in 1:64] + +bw = SA[1.0; 1.0; 1.0; 0.3; 0.3; 0.3] +mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw=bw,kernel=AMP.MvNormalKernel) + +cost4(σ) = begin + AMP.entropy(mtree, diagm(σ.^2)) +end + +# and optimize with "update" kernel bandwith cost +@time res = Optim.optimize( + cost4, + collect(bw), + Optim.NelderMead() +); + +@test res.ls_success + +@show best_cov = abs.(Optim.minimizer(res)) + +@test isapprox([0.75; 0.75; 0.75], best_cov[1:3]; atol=0.4) +@test isapprox([0.06; 0.06; 0.06], best_cov[4:6]; atol=0.04) + + +mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) + +@test isapprox([0.75 0 0; 0 0.75 0; 0 0 0.75], getBW(mkd)[1][1:3,1:3]; atol=0.4) +@test isapprox([0.06 0 0; 0 0.06 0; 0 0 0.06], getBW(mkd)[1][4:6,4:6]; atol=0.04) + +## +end + ## # # using GLMakie @@ -1064,4 +1102,4 @@ end # lines!((s->s[1]).(XX),YY, color=:red) -# \ No newline at end of file +# From 09b7da2f0ef4829a689ba7120f55bc7e0da2d676 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 20 May 2024 00:32:35 -0700 Subject: [PATCH 16/31] merge multidim multiscale Gibbs sampling --- src/CommonUtils.jl | 8 +++- src/services/ManellicTree.jl | 77 ++++++++++++++++++++++++++++--- test/manellic/testManellicTree.jl | 48 +++++++++++++++++-- 3 files changed, 121 insertions(+), 12 deletions(-) diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index a0f7574..e7cca5e 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -127,6 +127,7 @@ function calcProductGaussians( Σ_::Union{Nothing,<:AbstractVector{S},<:NTuple{N,S}}; dim::Integer=manifold_dimension(M), Λ_ = inv.(Σ_), # TODO these probably need to be transported to common tangent space `u0` -- FYI @Affie 24Q2 + weight::Real = 1.0 ) where {N,P,S<:AbstractMatrix{<:Real}} # # calc sum of covariances @@ -159,6 +160,7 @@ calcProductGaussians( Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; dim::Integer=manifold_dimension(M), Λ_ = map(s->diagm( 1.0 ./ s), Σ_), + weight::Real = 1.0 ) where {N,P,S<:AbstractVector} = calcProductGaussians(M, μ_, nothing; dim, Λ_=Λ_ ) # @@ -167,6 +169,7 @@ calcProductGaussians( μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}; dim::Integer=manifold_dimension(M), Λ_ = diagm.( (1.0 ./ μ_) ), + weight::Real = 1.0, ) where {N,P} = calcProductGaussians(M, μ_, nothing; dim, Λ_=Λ_ ) @@ -182,7 +185,8 @@ DevNotes """ function calcProductGaussians( M::AbstractManifold, - comps::AbstractVector{<:MvNormalKernel}, + comps::AbstractVector{<:MvNormalKernel}; + weight::Real = 1.0 ) # CHECK this should be on-manifold for points μ_ = mean.(comps) # This is a ArrayPartition which IS DEFINITELY ON MANIFOLD (we dispatch on mean) @@ -192,7 +196,7 @@ function calcProductGaussians( _μ, _Σ = calcProductGaussians(M, μ_, Σ_) - return MvNormalKernel(_μ, _Σ) + return MvNormalKernel(_μ, _Σ, weight) end diff --git a/src/services/ManellicTree.jl b/src/services/ManellicTree.jl index aa2ba8e..3fd8994 100644 --- a/src/services/ManellicTree.jl +++ b/src/services/ManellicTree.jl @@ -516,6 +516,66 @@ function buildTree_Manellic!( return tosort_leaves end +function buildTree_Manellic!( + M::AbstractManifold, + r_ker::AbstractVector{KL}; # vector of points referenced to the r_frame + N = length(r_ker), + weights::AbstractVector{<:Real} = ones(N).*(1/N), + kernel = KL, + kernel_bw = nothing, # TODO +) where {KL <: MvNormalKernel} + # + _μT() = typeof(r_ker[1].μ) + D = manifold_dimension(M) + CV = SMatrix{D,D,Float64,D*D}(collect(cov(r_ker[1]))) + KLT = getfield(ApproxManifoldProducts,kernel.name.name) + KT = KLT( + r_ker[1].μ, + CV + ) |> typeof + + r_PP = SizedVector{N,_μT()}(undef) + + # leaf kernels + lkern = SizedVector{N,KL}(undef) + _workaround_isdef_leafkernel = Set{Int}() + for i in 1:N + r_PP[i] = r_ker[i].μ + lkern[i] = if isnothing(kernel_bw) + r_ker[i] + else + updateKernelBW(r_ker[i], kernel_bw) # TODO handle vector of kernel_bws + end + push!(_workaround_isdef_leafkernel, i + N) + end + + mtree = ManellicTree( + M, + r_PP, + MVector{N,Float64}(weights), + MVector{N,Int}(1:N), + lkern, + SizedVector{N,KT}(undef), + SizedVector{N,Set{Int}}(undef), + _workaround_isdef_leafkernel, + Set{Int}(), + ) + + # + tosort_leaves = buildTree_Manellic!( + mtree, + 1, # start at root + 1, # spanning all data + N; # to end of data + kernel=KLT, + kernel_bw + ) + + # manual reset leaves in the order discovered + permute!(tosort_leaves.leaf_kernels, tosort_leaves.permute) + + return tosort_leaves +end function updateBandwidths( mtr::ManellicTree{M,D,N,HL}, @@ -719,7 +779,8 @@ function calcProductKernelBTLabels( labels_sampled, LOOidx::Union{Int, Nothing} = nothing, gibbsSeq = 1:length(proposals); - permute::Bool = true # true because signature is BTLabels + permute::Bool = true, # true because signature is BTLabels + weight::Real = 1.0 ) # select a density label from the other proposals prop_and_label = Tuple{Int,Int}[] @@ -732,7 +793,7 @@ function calcProductKernelBTLabels( components = map(pr_lb->getKernelTree(proposals[pr_lb[1]], pr_lb[2], permute, true), prop_and_label) # TODO upgrade to tuples - return calcProductGaussians(M, [components...]) + return calcProductGaussians(M, [components...]; weight) end @@ -740,14 +801,16 @@ function calcProductKernelsBTLabels( M::AbstractManifold, proposals::AbstractVector, N_lbl_sets::AbstractVector{<:NTuple}, - permute::Bool = true # true because signature is BTLabels + permute::Bool = true; # true because signature is BTLabels + weights = 1/length(N_lbl_sets) .* ones(length(N_lbl_sets)) ) # T = typeof(getKernelTree(proposals[1],1)) - post = Vector{T}(undef, length(N_lbl_sets)) + N = length(N_lbl_sets) + post = Vector{T}(undef, N) for (i,lbs) in enumerate(N_lbl_sets) - post[i] = calcProductKernelBTLabels(M, proposals, lbs; permute) + post[i] = calcProductKernelBTLabels(M, proposals, lbs; permute, weight=weights[i]) end return post @@ -852,12 +915,14 @@ function sampleProductSeqGibbsBTLabel( return labels_sampled end +Base.length(mkd::ManifoldKernelDensity) = length(mkd.belief) + function sampleProductSeqGibbsBTLabels( M::AbstractManifold, proposals::AbstractVector, MC = 3, - N::Int = round(Int, mean(length.(getPoints.(proposals)))), # FIXME use getLength or length of proposal (not getPoints) + N::Int = round(Int, mean(length.(proposals))), # FIXME use getLength or length of proposal (not getPoints) label_pools=[[1:1;] for _ in proposals] ) # diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 3675ae2..7db68fd 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -999,7 +999,7 @@ end # end -@testset "Product of two Manellic beliefs, Sequential Gibbs" begin +@testset "Product of two Manellic beliefs, Sequential Gibbs, TranslationGroup(1)" begin ## M = TranslationGroup(1) @@ -1034,7 +1034,7 @@ bt_label_pool = [ ] # leaves only version -@info "Leaves only label sampling version (Gibbs)" +@info "Leaves only label sampling version (Gibbs), TranslationGroup(1)" ApproxManifoldProducts.sampleProductSeqGibbsBTLabel(M, [p1; p2], 3, bt_label_pool) @@ -1047,10 +1047,10 @@ mtr = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw, kernel=Appro @test isapprox( 0, mean(ApproxManifoldProducts.getKernelTree(mtr,1))[1]; atol=0.75) +@test all( (s->isapprox(1/N, s.weight;atol=1e-6)).(post) ) - -@info "Multi-scale label sampling version (Gibbs)" +@info "Multi-scale label sampling version (Gibbs), TranslationGroup(1)" # test label pool creation child_label_pools, all_leaves = ApproxManifoldProducts.generateLabelPoolRecursive([p1;p2], [1; 1]) @@ -1102,4 +1102,44 @@ end # lines!((s->s[1]).(XX),YY, color=:red) + +@testset "Multi-scale label sampling version (Gibbs), TranslationGroup(2)" begin +## + +M = TranslationGroup(2) +N = 64 + +pts1 = [1*randn(2) for _ in 1:N] +p1 = ApproxManifoldProducts.manikde!_manellic(M,pts1) + +pts2 = [1*randn(2) for _ in 1:N] +p2 = ApproxManifoldProducts.manikde!_manellic(M,pts2) + + +# test sampling +lbls = ApproxManifoldProducts.sampleProductSeqGibbsBTLabels(M, [p1.belief; p2.belief]) +lbls_ = unique(lbls) +N_ = length(lbls_) +weights = 1/N .* ones(N_) +# increase weight of duplicates +if N_ < N + for (i,lb_) in enumerate(lbls_) + idxs = findall(==(lb_),lbls) + weights[i] = weights[i]*length(idxs) + end +end +post = ApproxManifoldProducts.calcProductKernelsBTLabels(M, [p1.belief; p2.belief], lbls_, false; weights) # ?? was permute=false? +# check that any duplicates resulted in a height weight +@test isapprox( weights, (s->s.weight).(post); atol=1e-6 ) + +# NOTE, resulting tree might not have N number of data points +mtr12 = ApproxManifoldProducts.buildTree_Manellic!(M,post) + +## +end + + + + + # From 94426c78751de359a4c565a5c713afe13d32c37a Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 20 May 2024 00:40:10 -0700 Subject: [PATCH 17/31] manellic bump test --- test/manellic/testManellicTree.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 7db68fd..e9e8b08 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -914,8 +914,8 @@ end mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) -@test isapprox([0.75 0 0; 0 0.75 0; 0 0 0.75], getBW(mkd)[1][1:3,1:3]; atol=0.4) -@test isapprox([0.06 0 0; 0 0.06 0; 0 0 0.06], getBW(mkd)[1][4:6,4:6]; atol=0.04) +@test isapprox([0.75 0 0; 0 0.75 0; 0 0 0.75], getBW(mkd)[1][1:3,1:3]; atol=0.5) +@test isapprox([0.07 0 0; 0 0.07 0; 0 0 0.07], getBW(mkd)[1][4:6,4:6]; atol=0.05) ## end @@ -951,7 +951,7 @@ end ## -@testset "Test utility functions for Gaussian products" begin +@testset "Test utility functions for Gaussian products, TranslationGroup(1)" begin ## M = TranslationGroup(1) From a05cbc65b705223b7b316b6ab903720fa506d7d5 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 20 May 2024 01:11:49 -0700 Subject: [PATCH 18/31] adj manellic test criteria --- test/manellic/testManellicTree.jl | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index e9e8b08..cb347bd 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -37,7 +37,7 @@ function testEigenCoords( # spot check @show _ax_ERR = log_lie(SpecialOrthogonal(2), (r_R_ax_')*r_R_ax)[1,2] - @show testval = isapprox(0, _ax_ERR; atol = 6/length(ax_CC)) + @show testval = isapprox(0, _ax_ERR; atol = 8/length(ax_CC)) @assert testval "Spot check failed on eigen split of manifold points, the estimated point rotation matrix did not match construction. length(ax_CC)=$(length(ax_CC))" r_CC, r_R_ax_, pidx, r_CV @@ -435,8 +435,11 @@ mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = diagm([0. p = exp(M, ϵ, hat(M, ϵ, [10, 20, 0.1])) y_amp = AMP.evaluate(mtree, p) y_pdf = pdf(dis, [10,20,0.1]) -#FIXME -@test_broken isapprox(y_amp, y_pdf; atol=0.1) +# check kde eval is within 40% of true value +y_err = y_amp - y_pdf +@show y_pdf +@test isapprox(0, y_err; atol=0.4*y_pdf) +@warn "weak test for approx function vs. true Normal density function evaluation" end @@ -510,7 +513,7 @@ end amp_bf_pqs = amp_ps .* amp_qs -#FIXME +#FIXME -- brute force will be more accurate than approx product, relax these tests for stochastic variability normalized_compare_test = isapprox.(normalize(amp_pqs), normalize(amp_bf_pqs); atol=0.001) @test_broken all(normalized_compare_test) @warn "Brute force product test overlap $(round(count(normalized_compare_test) / length(amp_pqs) * 100, digits=2))%" From 98aca3b6d502b5de8df72239fb8ec8975ab15d66 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 20 May 2024 01:16:56 -0700 Subject: [PATCH 19/31] improve manellic test --- test/manellic/testManellicTree.jl | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index cb347bd..fe3a669 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -435,11 +435,13 @@ mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = diagm([0. p = exp(M, ϵ, hat(M, ϵ, [10, 20, 0.1])) y_amp = AMP.evaluate(mtree, p) y_pdf = pdf(dis, [10,20,0.1]) -# check kde eval is within 40% of true value +# check kde eval is within 20% of true value y_err = y_amp - y_pdf @show y_pdf -@test isapprox(0, y_err; atol=0.4*y_pdf) -@warn "weak test for approx function vs. true Normal density function evaluation" +if !isapprox(0, y_err; atol=0.2*y_pdf) + @warn "soft test failure for approx function vs. true Normal density function evaluation" + @test_broken false +end end From 047d81debd918833873c0c1d1dacefbba9345b04 Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Mon, 20 May 2024 15:53:12 -0700 Subject: [PATCH 20/31] bump test manellic stochastic --- test/manellic/testManellicTree.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index fe3a669..1c8dc23 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -880,7 +880,7 @@ end mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) -@test isapprox([0.6 0; 0 0.6], getBW(mkd)[1][1:2,1:2]; atol=0.35) +@test isapprox([0.6 0; 0 0.6], getBW(mkd)[1][1:2,1:2]; atol=0.4) @test isapprox(0.06, getBW(mkd)[1][3,3]; atol=0.04) From 6c9e81dbd660c7b0dc55cc2a9da5c5544c405a80 Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Mon, 15 Jul 2024 12:36:16 -0400 Subject: [PATCH 21/31] Towards Mahony 2024 on-manifold Gaussian products (#295) * Towards Mahony 2024 on-manifold Gaussian products Co-authored-by: Johannes Terblanche * restore gau prod test for JL11 * refac mani overloads, more adjs --------- Co-authored-by: Johannes Terblanche --- Project.toml | 2 +- src/ApproxManifoldProducts.jl | 45 +++-- src/CommonUtils.jl | 12 +- src/services/ManifoldsOverloads.jl | 283 +++++++++++++++++++++++++++++ test/manellic/testManellicTree.jl | 127 +++++++++---- test/runtests.jl | 1 + test/testLieFundamentals.jl | 161 ++++++++++++++++ 7 files changed, 577 insertions(+), 54 deletions(-) create mode 100644 src/services/ManifoldsOverloads.jl create mode 100644 test/testLieFundamentals.jl diff --git a/Project.toml b/Project.toml index 5546e42..3b4db3c 100644 --- a/Project.toml +++ b/Project.toml @@ -53,7 +53,7 @@ Rotations = "1" StaticArrays = "0.15, 1" Statistics = "1" TensorCast = "0.2, 0.3, 0.4" -TransformUtils = "0.2.10" +TransformUtils = "0.2.17" julia = "1.9" [extensions] diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index c6e8b8f..5c5d1a3 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -1,39 +1,45 @@ module ApproxManifoldProducts -using Reexport -@reexport using KernelDensityEstimate +import Base: *, isapprox, convert, show, eltype, length + +using Logging +using StaticArrays +using LinearAlgebra +import LinearAlgebra: rotate!, det + +using TensorCast +using DocStringExtensions +using Distributions using Random +import Random: rand + +using Statistics +import Statistics: mean, std, cov, var, entropy -import TransformUtils as TUs import Rotations as _Rot +using CoordinateTransformations -import ManifoldsBase -import ManifoldsBase: AbstractManifold, distance using RecursiveArrayTools: ArrayPartition export ArrayPartition +import ManifoldsBase +import ManifoldsBase: AbstractManifold, distance using Manifolds -using DocStringExtensions - using NLsolve import Optim -using CoordinateTransformations -using Requires -using LinearAlgebra -using TensorCast -using StaticArrays -using Logging -using Statistics -using Distributions -import Random: rand +import TransformUtils as TUs +import TransformUtils: skew -import Base: *, isapprox, convert, show, eltype, length -import LinearAlgebra: rotate!, det -import Statistics: mean, std, cov, var, entropy +using Reexport +@reexport using KernelDensityEstimate import KernelDensityEstimate: getPoints, getBW, evalAvgLogL, entropy, evaluate + +# FIXME ON FIRE OBSOLETE REMOVE +using Requires + const MB = ManifoldsBase const CTs = CoordinateTransformations const AMP = ApproxManifoldProducts @@ -52,6 +58,7 @@ include("entities/KernelEval.jl") include("entities/ManellicTree.jl") # experimental include("entities/ManifoldKernelDensity.jl") +include("services/ManifoldsOverloads.jl") include("CommonUtils.jl") include("services/ManellicTree.jl") diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index e7cca5e..32d0233 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -104,6 +104,8 @@ end # end + + """ $SIGNATURES @@ -132,8 +134,11 @@ function calcProductGaussians( # # calc sum of covariances Λ = zeros(MMatrix{dim,dim}) + # FIXME first transport (push forward) covariances to common coordinates + # see [Ge, van Goor, Mahony, 2024] Λ .= sum(Λ_) + # naive mean # Tangent space reference around the evenly weighted mean of incoming points u0 = mean(M, μ_) @@ -144,7 +149,7 @@ function calcProductGaussians( Δuvee = vee(M, u0, log(M, u0, u)) ΛΔμ += s*Δuvee end - + Λ # calculate the delta mean Δμ = Λ \ ΛΔμ @@ -179,8 +184,9 @@ calcProductGaussians( EXPERIMENTAL: On-manifold product of Gaussians. DevNotes -- CHECK make sure this product is properly on manifold, Manifolds.jl likely already has solutions: - - https://juliamanifolds.github.io/Manifolds.jl/stable/features/distributions.html +- FIXME do product of concentrated Gaussians on Lie group (approximation): + - See Section 3.2 and 4 of [Ge, van Goor, Mahony: A Geometric Perspective on using Gaussian Distributions on Lie Groups, 2024]. + - Also see upstream utils, https://juliamanifolds.github.io/Manifolds.jl/stable/features/distributions.html - FIXME is parallel transport needed when multiplying with covariances from difffent tangent spaces? """ function calcProductGaussians( diff --git a/src/services/ManifoldsOverloads.jl b/src/services/ManifoldsOverloads.jl new file mode 100644 index 0000000..eecd291 --- /dev/null +++ b/src/services/ManifoldsOverloads.jl @@ -0,0 +1,283 @@ + +const _UPSTREAM_MANIFOLDS_ADJOINT_ACTION = false + +# local union definition during development -- TODO consolidate upstream +LieGroupManifoldsPirate = Union{typeof(SpecialOrthogonal(2)), typeof(SpecialOrthogonal(3)), typeof(SpecialEuclidean(2)), typeof(SpecialEuclidean(3))} + +## ================================ GENERIC IMPLEMENTATIONS ================================ + + +function TUs.skew(v::SVector{3,T}) where T<:Real + x,y,z = v[1], v[2], v[3] + return SMatrix{3,3,T}( + +0, + z, + -y, + -z, + 0, + x, + +y, + -x, + 0 + ) +end + + +# right Jacobian (Lie Group, originally from ?) +function Jr(M::Manifolds.GroupManifold, X; order=5) + adx = ad(M, X) + mapreduce(+, 0:order) do i + (-adx)^i / factorial(i + 1) + end +end + +# "The left trivialized Jacobian is referred to as the right Jacobian in much of the key literature." [Ge, van Goor, Mahony, 2024] +# argument d is a Lie algebra element giving the direction of transport +function Jl_trivialized( + M::Manifolds.GroupManifold, + d +) + adM = ad(M,d) + ead = exp(-adM) + adM \ (LinearAlgebra.I - ead) +end + + +""" + $SIGNATURES + +EXPERIMENTAL: ApproxManifoldProducts hard coded versions for best parallel transport available (as per development cycle). + +Inputs: +- M is a Manifold (must be a Lie group) +- p is the expansion point on manifold +- X is a tangent vector which is to be transported +- d is a tangent vector from a starting point in the direction and distance to transport + +Sidenote on why lowercase tangent vector `d` (as driven by Manifolds.jl) +``` +so(2) +X = Xc*e1 = Xc*î +î = ∂/∂x = [0 -1; 0 1] # orthogonal basis +``` +Notes +- Default is transport without curvature estimate provided by upstream Manifolds.jl + +See also: [`Jl_trivialized'](@ref), [`Jr`](@ref), `Manifolds.parallel_transport_direction`, `Manifolds.parallel_transport_to`, `Manifolds.parallel_transport_along` +""" +parallel_transport_best( + M::AbstractManifold, + p, + X::AbstractArray, + d::AbstractArray +) = Manifolds.parallel_transport_direction(M, p, X, d) + + + +## ================================= Lie (a/A)djoints ================================= +## ---------------------------------- Almost generic ---------------------------------- + +# assumes inputs are Lie algebra tangent vectors represented in matrix form +ad( + M::LieGroupManifoldsPirate, + A::AbstractMatrix, + B::AbstractMatrix +) = Manifolds.lie_bracket(M, A, B) + + +Ad( + M::Union{typeof(SpecialOrthogonal(2)), typeof(SpecialOrthogonal(3))}, + p, + X::AbstractMatrix; + use_upstream::Bool = _UPSTREAM_MANIFOLDS_ADJOINT_ACTION # explict to support R&D +) = if use_upstream + Manifolds.adjoint_action(M, p, X) +else + p*X*(p') +end + + +function Ad( + M::Union{typeof(SpecialEuclidean(2)), typeof(SpecialEuclidean(3))}, + p, + X::ArrayPartition; + use_upstream::Bool = _UPSTREAM_MANIFOLDS_ADJOINT_ACTION # explict to support R&D +) + if use_upstream + # TODO swap and test + Manifolds.adjoint_action(M, p, X) + else + t = p.x[1] + R = p.x[2] + v = X.x[1] + Ω = X.x[2] + # direct adjoint for SO(.) + RΩR = Ad(M.manifold[2], R, Ω; use_upstream ) + ArrayPartition(-RΩR*t + R*v, RΩR) + end +end + + +# left trivialized Jacobian [Mahony, 2024, Theorem 4.3, matrix P_u_vee] +# U is direction in which to transport along, given as Lie algebra element (ie tangent vector) +# this produces the transportMatrix P_u^vee, per [Mahony, 2024] +# d is a Lie algebra element giving the direction of transport +parallel_transport_direction_lie( + M::typeof(SpecialOrthogonal(3)), + d::AbstractMatrix +) = exp(ad(M, -0.5*d)) + + +# transport with 2nd order curvature approximation +# left trivialized Jacobian [Mahony, 2024, Theorem 4.3] +parallel_transport_along_2nd( + M::LieGroupManifoldsPirate, + p, + X::AbstractMatrix, + d::AbstractMatrix +) = Jl_trivialized(M, d) * vee(M,p,X) + + +parallel_transport_best( + M::LieGroupManifoldsPirate, + p, + X::AbstractMatrix, + d::AbstractMatrix +) = parallel_transport_along_2nd(M,p,X,d) + +## ----------------------------- SpecialOrthogonal(3) ----------------------------- + + +# matrix versions +# [Chirikjian, 2012 Vol2, p.39] +ad( + ::typeof(SpecialOrthogonal(3)), + X +) = X + +Ad( + ::typeof(SpecialOrthogonal(3)), + R +) = R + + +## For SO(3) Jr and Jl + inv closed forms, see [Chirikjian 2012, Vol2, Vol2 p.40] !!! + + +# Matrix form parallel transport with curvature correction (2nd order) +# left trivialized Jacobian [Mahony, 2024, Theorem 4.3] +# U is direction in which to transport (w/ 2nd order along curvature correction), +# Direction d is a Lie algebra element (tangent vector) providing direction of transport +function Jl_trivialized( + M::typeof(SpecialOrthogonal(3)), + d +) + aduv = ad(M, -0.5*d) + P = exp(aduv) + return P*(LinearAlgebra.I + 1/24*aduv^2) +end + +# d: Lie algebra for the direction of transport +# Xc are coordinates to be transported +Jl_trivialized( + M::typeof(SpecialOrthogonal(3)), + d::AbstractMatrix, + Xc::AbstractVector +) = Jl_trivialized(M, d)*Xc + + + +function parallel_transport_direction_lie( + M::typeof(SpecialOrthogonal(3)), + d::AbstractMatrix, + X +) + hat( + M, + Identity(M), + parallel_transport_direction_lie(M, d) * vee(M, Identity(M), X) + ) +end + + + +## ----------------------------- SpecialEuclidean(.) ----------------------------- + + +# assumes inputs are Lie algebra tangent vectors represented in (Array) Partion form +# d is a Lie algebra element (tangent vector) providing the direction of transport +# X is the tangent vector to be transported +function ad( + M::Union{typeof(SpecialEuclidean(2)), typeof(SpecialEuclidean(3))}, + d::ArrayPartition, + X::ArrayPartition +) + # + v1x = skew(d.x[1]) + Ω1 = d.x[2] + v2 = X.x[1] + ω2 = log_lie(M.manifold[2], X.x[2]) + + Rε = identity_element(M.manifold[2], Ω1) + Ω = Manifolds.hat(M.manifold[2], Rε, Ω1*ω2) + + ArrayPartition(v1x*ω2 + Ω1*v2, Ω) +end + + +# matrix versions + + +function ad( + ::typeof(SpecialEuclidean(2)), + d::ArrayPartition, +) + Vx = SA[d.x[2]; -d.x[1]] + Ω = d.x[2] + vcat( + hcat(Ω, Vx), + zero(SMatrix{1,3,Float64}) + ) +end + +function ad( + ::typeof(SpecialEuclidean(3)), + d::ArrayPartition, +) + Vx = skew(d.x[1]) + Ω = d.x[2] + vcat( + hcat(Ω, Vx), + hcat(zero(SMatrix{3,3,Float64}), Ω) + ) +end + + +function Ad( + ::typeof(SpecialEuclidean(2)), + p +) + t = p.x[1] + R = p.x[2] + vcat( + hcat(R, -SA[0 -1; 1 0]*t), + SA[0 0 1] + ) +end + +function Ad( + ::typeof(SpecialEuclidean(3)), + p +) + t = p.x[1] + R = p.x[2] + vcat( + hcat(R, skew(t)*R), + hcat(zero(SMatrix{3,3,Float64}), R) + ) +end + + + + +# \ No newline at end of file diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index 1c8dc23..fbaed25 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -310,9 +310,9 @@ ker = AMP.MvNormalKernel([0], [2.0;;]) ## M = SpecialEuclidean(2) -ϵ = identity_element(M) +ε = identity_element(M) Xc = [10, 20, 0.1] -p = exp(M, ϵ, hat(M, ϵ, Xc)) +p = exp(M, ε, hat(M, ε, Xc)) kercov = diagm([0.5, 2.0, 0.1].^2) ker = AMP.MvNormalKernel(p, kercov) @test isapprox( @@ -321,15 +321,15 @@ ker = AMP.MvNormalKernel(p, kercov) ) Xc = [10, 22, -0.1] -q = exp(M, ϵ, hat(M, ϵ, Xc)) +q = exp(M, ε, hat(M, ε, Xc)) @test isapprox( pdf(MvNormal(cov(ker)), [0,0,0]), AMP.evaluate(M, ker, p) ) -X = log(M, ϵ, Manifolds.compose(M, inv(M, p), q)) -Xc_e = vee(M, ϵ, X) +X = log(M, ε, Manifolds.compose(M, inv(M, p), q)) +Xc_e = vee(M, ε, X) pdf_local_coords = pdf(MvNormal(cov(ker)), Xc_e) @test isapprox( @@ -338,8 +338,8 @@ pdf_local_coords = pdf(MvNormal(cov(ker)), Xc_e) ) delta_c = AMP.distanceMalahanobisCoordinates(M, ker, q) -X = log(M, ϵ, Manifolds.compose(M, inv(M, p), q)) -Xc_e = vee(M, ϵ, X) +X = log(M, ε, Manifolds.compose(M, inv(M, p), q)) +Xc_e = vee(M, ε, X) malad_t = Xc_e'*inv(kercov)*Xc_e # delta_t = [10, 20, 0.1] - [10, 22, -0.1] @test isapprox( @@ -365,7 +365,7 @@ rbfd = AMP.ker(M, ker, q, 0.5, AMP.distanceMalahanobisSq) # NOTE 'global' distribution would have been X = log(M, mean(ker), q) -Xc_e = vee(M, ϵ, X) +Xc_e = vee(M, ε, X) pdf_global_coords = pdf(MvNormal(cov(ker)), Xc_e) @@ -376,14 +376,14 @@ end M = TranslationGroup(1) -ϵ = identity_element(M) +ε = identity_element(M) dis = MvNormal([3.0], diagm([1.0].^2)) Cpts = [rand(dis) for _ in 1:128] -pts = map(c->exp(M, ϵ, hat(M, ϵ, c)), Cpts) +pts = map(c->exp(M, ε, hat(M, ε, c)), Cpts) mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = [0.2;;], kernel=AMP.MvNormalKernel) ## -p = exp(M, ϵ, hat(M, ϵ, [3.0])) +p = exp(M, ε, hat(M, ε, [3.0])) y_amp = AMP.evaluate(mtree, p) y_pdf = pdf(dis, [3.0]) @@ -391,7 +391,7 @@ y_pdf = pdf(dis, [3.0]) @test isapprox(y_amp, y_pdf; atol=0.1) # ps = [[p] for p = -0:0.01:6] -# ys_amp = map(p->AMP.evaluate(mtree, exp(M, ϵ, hat(M, ϵ, p))), ps) +# ys_amp = map(p->AMP.evaluate(mtree, exp(M, ε, hat(M, ε, p))), ps) # ys_pdf = pdf(dis, ps) # lines(first.(ps), ys_pdf) @@ -402,14 +402,14 @@ y_pdf = pdf(dis, [3.0]) ## M = SpecialOrthogonal(2) -ϵ = identity_element(M) +ε = identity_element(M) dis = MvNormal([0.0], diagm([0.1].^2)) Cpts = [rand(dis) for _ in 1:128] -pts = map(c->exp(M, ϵ, hat(M, ϵ, c)), Cpts) +pts = map(c->exp(M, ε, hat(M, ε, c)), Cpts) mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = [0.005;;], kernel=AMP.MvNormalKernel) ## -p = exp(M, ϵ, hat(M, ϵ, [0.1])) +p = exp(M, ε, hat(M, ε, [0.1])) y_amp = AMP.evaluate(mtree, p) y_pdf = pdf(dis, [0.1]) @@ -417,7 +417,7 @@ y_pdf = pdf(dis, [0.1]) @test isapprox(y_amp, y_pdf; atol=0.5) ps = [[p] for p = -0.3:0.01:0.3] -ys_amp = map(p->AMP.evaluate(mtree, exp(M, ϵ, hat(M, ϵ, p))), ps) +ys_amp = map(p->AMP.evaluate(mtree, exp(M, ε, hat(M, ε, p))), ps) ys_pdf = pdf(dis, ps) # lines(first.(ps), ys_pdf) @@ -425,14 +425,14 @@ ys_pdf = pdf(dis, ps) M = SpecialEuclidean(2) -ϵ = identity_element(M) +ε = identity_element(M) dis = MvNormal([10,20,0.1], diagm([0.5,2.0,0.1].^2)) Cpts = [rand(dis) for _ in 1:128] -pts = map(c->exp(M, ϵ, hat(M, ϵ, c)), Cpts) +pts = map(c->exp(M, ε, hat(M, ε, c)), Cpts) mtree = ApproxManifoldProducts.buildTree_Manellic!(M, pts; kernel_bw = diagm([0.05,0.2,0.01]), kernel=AMP.MvNormalKernel) ## -p = exp(M, ϵ, hat(M, ϵ, [10, 20, 0.1])) +p = exp(M, ε, hat(M, ε, [10, 20, 0.1])) y_amp = AMP.evaluate(mtree, p) y_pdf = pdf(dis, [10,20,0.1]) # check kde eval is within 20% of true value @@ -447,17 +447,17 @@ end ## ======================================================================================== -@testset "Test Product the brute force way" begin +@testset "Test Product the brute force way, SpecialEuclidean(2)" begin M = SpecialEuclidean(2) -ϵ = identity_element(M) +ε = identity_element(M) Xc_p = [10, 20, 0.1] -p = exp(M, ϵ, hat(M, ϵ, Xc_p)) +p = exp(M, ε, hat(M, ε, Xc_p)) kerp = AMP.MvNormalKernel(p, diagm([0.5, 2.0, 0.1].^2)) Xc_q = [10, 22, -0.1] -q = exp(M, ϵ, hat(M, ϵ, Xc_q)) +q = exp(M, ε, hat(M, ε, Xc_q)) kerq = AMP.MvNormalKernel(q, diagm([1.0, 1.0, 0.1].^2)) kerpq = calcProductGaussians(M, [kerp, kerq]) @@ -468,7 +468,7 @@ ys = 15:0.1:27 θs = -0.3:0.01:0.3 grid_points = map(Iterators.product(xs, ys, θs)) do (x,y,θ) - exp(M, ϵ, hat(M, ϵ, SVector(x,y,θ))) + exp(M, ε, hat(M, ε, SVector(x,y,θ))) end # use_global_coords = true @@ -477,11 +477,11 @@ use_global_coords = false pdf_ps = map(grid_points) do gp if use_global_coords X = log(M, p, gp) - Xc_e = vee(M, ϵ, X) + Xc_e = vee(M, ε, X) pdf(MvNormal(cov(kerp)), Xc_e) else - X = log(M, ϵ, Manifolds.compose(M, inv(M, p), gp)) - Xc_e = vee(M, ϵ, X) + X = log(M, ε, Manifolds.compose(M, inv(M, p), gp)) + Xc_e = vee(M, ε, X) pdf(MvNormal(cov(kerp)), Xc_e) end end @@ -489,11 +489,11 @@ end pdf_qs = map(grid_points) do gp if use_global_coords X = log(M, q, gp) - Xc_e = vee(M, ϵ, X) + Xc_e = vee(M, ε, X) pdf(MvNormal(cov(kerq)), Xc_e) else - X = log(M, ϵ, Manifolds.compose(M, inv(M, q), gp)) - Xc_e = vee(M, ϵ, X) + X = log(M, ε, Manifolds.compose(M, inv(M, q), gp)) + Xc_e = vee(M, ε, X) pdf(MvNormal(cov(kerq)), Xc_e) end end @@ -576,10 +576,75 @@ normalized_compare_test = isapprox.(normalize(amp_pqs), normalize(amp_bf_pqs); a # lines!(θs, pdf_q) # lines!(θs, pdf_pq) +## +end + + +@testset "Rotated covariance product major axis checks, TranslationGroup(2)" begin +## + +M = TranslationGroup(2) +ε = identity_element(M) + +Xc_p = [0, 0.0] +p = exp(M, ε, hat(M, ε, Xc_p)) +kerp = AMP.MvNormalKernel(p, diagm([2.0, 1.0].^2)) + +Xc_q = [0, 0.0] +# rotate by 60 deg +R = Rot_.RotMatrix{2}(pi/3).mat +Σ = R * diagm([2.0, 1.0].^2) * R' +q = exp(M, ε, hat(M, ε, Xc_q)) +kerq = AMP.MvNormalKernel(q, Σ) +kerpq = calcProductGaussians(M, [kerp, kerq]) + +evv = eigen(cov(kerpq)) +maj_idx = sortperm(evv.values)[end] + +# check that the major axis is halfway between 0 and 60deg -- i.e. 30 deg +maj_ang = (angle(Complex(evv.vectors[:,maj_idx]...)) + 2pi) % pi +@test isapprox(pi/180*30, maj_ang; atol = 1e-8) + +## end -## ======================================================================================== + +@testset "Rotated covariance product major axis checks, SpecialEuclidean(2)" begin +## + +M = SpecialEuclidean(2) +ε = identity_element(M) + +Xc_p = [0, 0, 0.0] +p = exp(M, ε, hat(M, ε, Xc_p)) +kerp = AMP.MvNormalKernel(p, diagm([2.0, 1.0, 0.1].^2)) + +# referenced to "global frame" +# rotate by 60 deg +Xc_q = [0, 0, pi/3] +q = exp(M, ε, hat(M, ε, Xc_q)) +kerq = AMP.MvNormalKernel(q, diagm([2.0, 1.0, 0.1].^2)) + +kerpq = calcProductGaussians(M, [kerp, kerq]) + +evv = eigen(cov(kerpq)) +maj_idx = sortperm(evv.values)[end] + +# check that the major axis is halfway between 0 and 60deg -- i.e. 30 deg +@show mean(kerpq) + +@test isapprox( + pi/6, + Rot_.RotMatrix(SMatrix{2,2,Float64}(mean(kerpq).x[2])) |> Rot_.rotation_angle; + atol=1e-8 +) + + +## +end + + @testset "Manellic basic evaluation test 1D" begin diff --git a/test/runtests.jl b/test/runtests.jl index c2f1c46..de4f6fc 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -7,6 +7,7 @@ include("basics.jl") include("ex_1D.jl") include("ex_2D_rot.jl") include("testManifoldConventions.jl") +include("testLieFundamentals.jl") include("testManifoldPartial.jl") include("testManiProductBigSmall.jl") include("testBasicManiProduct.jl") diff --git a/test/testLieFundamentals.jl b/test/testLieFundamentals.jl new file mode 100644 index 0000000..9c5b957 --- /dev/null +++ b/test/testLieFundamentals.jl @@ -0,0 +1,161 @@ + +using Test +using Manifolds +using ApproxManifoldProducts +using LinearAlgebra + + +## + + +# [Chirikjian 2012, Vol2, Vol2 p.40], specifically for SO(3) +# @test isapprox(Jl, R*Jr) +# @test isapprox(Jl*inv(Jr), R) == [Ad(R)] +# @test isapprox(Jl, Jr') +# @test det(Jl) == det(Jr) == 2*(1-cos||x||)/(||x||^2) + + +@testset "SO(3) Vector transport (w curvature) via Jacobians and Lie adjoints" begin +## + +M = SpecialOrthogonal(3) +p = Identity(M) +Xc = [1.,0,0] +X = hat(M, p, Xc) # random algebra element +d̂ = [0,0,1.] +d = hat(M, p, d̂) # direction in algebra + +## piggy back utility tests used in construction of transports + +# Ad_vee and ad_vee are matrices which use multiplication to operate Ad and ad respectively +# @test Ad_vee(exp_G(-0.5*coord_u)) == exp(-0.5*ad_vee(coord_u)) +@test isapprox( + ApproxManifoldProducts.Ad(M, exp_lie(M,-0.5*X)), + exp(-0.5*ApproxManifoldProducts.ad(M, X)) +) + +## parallel transport without curvature correction + +Y = parallel_transport_direction(M, p, X, d) # compare to Lie ad and Ad and P, bottom [Mahony 2024 p3] +Jl_trv_P = ApproxManifoldProducts.parallel_transport_direction_lie(M, d) +Yc_ = Jl_trv_P * Xc +Y_ = hat(M, p, Yc_) + +@test isapprox( + Y, + Y_ +) +@test isapprox( + Y, + ApproxManifoldProducts.parallel_transport_direction_lie(M, d, X) +) + +# @test isapprox(Jl*inv(Jr), R) == [Ad(R)] +Jr = Jl_trv_P +Jl = Jl_trv_P' +@test isapprox( + Jl*inv(Jr), + ApproxManifoldProducts.Ad(M,exp_lie(M,d)); + atol=1e-8 +) + +# @test det(Jl) == det(Jr) == 2*(1-cos||x||)/(||x||^2) # from [Chirikjian 2012, Vol 2, ~pg.40] +@test isapprox( + det(Jl), + det(Jr) +) +# @test isapprox( +# det(Jl), +# 2*(1-cos(norm(d̂)))/(norm(d̂)^2); +# atol=1e-8 +# ) + +## With curvature correction + +# compute transported coordinates (with Mahony 2nd order curvature correction) +# is this also a push-forward +Jl_trv = ApproxManifoldProducts.Jl_trivialized(M, d) +_Y_ = Jl_trv*Xc +# _Y_ = Jl_trivialized(M, d, Xc) + +# Approx check for approx curvature correctin _Y_ vs. transport in direction (wo curvature corr) Y +@test isapprox(_Y_, vee(M, p, Y); atol=1e-1) + +# @test isapprox(Jl*inv(Jr), R) == [Ad(R)] +Jr = Jl_trv +Jl = Jl_trv' +@test isapprox( + Jl*inv(Jr), + ApproxManifoldProducts.Ad(M,exp_lie(M,d)); + atol=1e-8 +) + +# @test det(Jl) == det(Jr) == 2*(1-cos||x||)/(||x||^2) # from [Chirikjian 2012, Vol 2, ~pg.40] +@test isapprox( + det(Jl), + det(Jr) +) +# @test isapprox( +# det(Jl), +# 2*(1-cos(norm(d̂)))/(norm(d̂)^2); +# atol=1e-8 +# ) + + +# ----- turnkey version ----- + +@test isapprox( + _Y_, + ApproxManifoldProducts.parallel_transport_best(M,p,X,d); + atol=1e-8 +) + + + +@warn "TODO find Manifolds operation equivalent to Mahony 2024, perhaps `vector_transport_along`?" +# # https://juliamanifolds.github.io/ManifoldsBase.jl/stable/vector_transports/ +# # https://juliamanifolds.github.io/ManifoldsBase.jl/stable/vector_transports/#ManifoldsBase.SchildsLadderTransport +# c = [0,0,1.] +# vector_transport_along(M, identity_element(M), X, c) +# vector_transport_direction(M, p, X, d) + +# q = exp(M, p, d) +# vector_transport_to(M, p, X, q) + +# c = mid_point(M, q, d) + +# vector_transport_direction(M, p, X, vee(M,p,c)) + + +## +end + + +@testset "SE(3) Vector transport (w curvature) via Jacobians and Lie adjoints" begin +## + + +M = SpecialEuclidean(3) +p = Identity(M) +Xc = 0.5*randn(6) +X = hat(M, p, Xc) # random algebra element +d̂ = 0.5*randn(6) +d = hat(M, p, d̂) # direction in algebra + +Jl_trv = ApproxManifoldProducts.Jl_trivialized(M, d) + +@error "Missing SE(3) vector transport numerical verify tests" +# # @test isapprox(Jl*inv(Jr), R) == [Ad(R)] +# Jr = Jl_trv +# Jl = Jl_trv' +# @test isapprox( +# Jl*inv(Jr), +# ApproxManifoldProducts.Ad(M,exp_lie(M,d)); +# atol=1e-5 +# ) + + +## +end + +# \ No newline at end of file From 0edc15b44518d369be5279c8f263c60c86e018d2 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 16 Jul 2024 03:00:07 -0700 Subject: [PATCH 22/31] impr generic adjoint matrix for Lie groups, cleanup --- src/ApproxManifoldProducts.jl | 2 +- src/services/ManifoldsOverloads.jl | 216 ++++++++++++++++++++--------- test/testLieFundamentals.jl | 104 ++++++++++++-- 3 files changed, 244 insertions(+), 78 deletions(-) diff --git a/src/ApproxManifoldProducts.jl b/src/ApproxManifoldProducts.jl index 5c5d1a3..6c7da77 100644 --- a/src/ApproxManifoldProducts.jl +++ b/src/ApproxManifoldProducts.jl @@ -14,7 +14,7 @@ using Random import Random: rand using Statistics -import Statistics: mean, std, cov, var, entropy +import Statistics: mean, std, cov, var # , entropy? 24Q3, # JL v1.11-rc1, Statistics v1.11.1. import Rotations as _Rot using CoordinateTransformations diff --git a/src/services/ManifoldsOverloads.jl b/src/services/ManifoldsOverloads.jl index eecd291..8c0c124 100644 --- a/src/services/ManifoldsOverloads.jl +++ b/src/services/ManifoldsOverloads.jl @@ -2,13 +2,70 @@ const _UPSTREAM_MANIFOLDS_ADJOINT_ACTION = false # local union definition during development -- TODO consolidate upstream -LieGroupManifoldsPirate = Union{typeof(SpecialOrthogonal(2)), typeof(SpecialOrthogonal(3)), typeof(SpecialEuclidean(2)), typeof(SpecialEuclidean(3))} +LieGroupManifoldsPirate = Union{ + typeof(SpecialOrthogonal(2)), + typeof(SpecialOrthogonal(3)), + typeof(SpecialEuclidean(2)), + typeof(SpecialEuclidean(3)) +} + +## ===================================== BASIS PIRATES ===================================== + +# Sidenote on why lowercase tangent vector `d` (as driven by Manifolds.jl) +# ``` +# so(2) +# X = Xc*e1 = Xc*î +# î = ∂/∂α = [0 -1; 0 1] # orthogonal basis +# ``` + +get_basis_affine( + ::TranslationGroup{Manifolds.TypeParameter{Tuple{N}}, ℝ} +) where N = map( + i->SVector{N,Float64}( ntuple(s->float(s==i),N) ), + 1:N +) + +get_basis_affine( + ::typeof(SpecialOrthogonal(2)) +) = tuple( + SA[0 -1; 1 0.0], +) + +get_basis_affine( + ::typeof(SpecialOrthogonal(3)) +) = tuple( + SA[0 -0 0; 0 0 -1; -0 1 0.0], + SA[0 -0 1; 0 0 -0; -1 0 0.0], + SA[0 -1 0; 1 0 -0; -0 0 0.0], +) + +get_basis_affine( + ::typeof(SpecialEuclidean(2)) +) = tuple( + SA[0 -0 1; 0 0 0; 0 0 0.0], + SA[0 -0 0; 0 0 1; 0 0 0.0], + SA[0 -1 0; 1 0 0; 0 0 0.0], +) + +get_basis_affine( + ::typeof(SpecialEuclidean(3)) +) = tuple( + SA[0 -0 0 1; 0 0 -0 0; -0 0 0 0; 0 0 0 0.0], + SA[0 -0 0 0; 0 0 -0 1; -0 0 0 0; 0 0 0 0.0], + SA[0 -0 0 0; 0 0 -0 0; -0 0 0 1; 0 0 0 0.0], + SA[0 -0 0 0; 0 0 -1 0; -0 1 0 0; 0 0 0 0.0], + SA[0 -0 1 0; 0 0 -0 0; -1 0 0 0; 0 0 0 0.0], + SA[0 -1 0 0; 1 0 -0 0; -0 0 0 0; 0 0 0 0.0], +) + ## ================================ GENERIC IMPLEMENTATIONS ================================ function TUs.skew(v::SVector{3,T}) where T<:Real + # coordinates are the co-tangent elements x,y,z = v[1], v[2], v[3] + # sum with default basis to form a tangent vector in the algebra return SMatrix{3,3,T}( +0, z, @@ -24,23 +81,28 @@ end # right Jacobian (Lie Group, originally from ?) -function Jr(M::Manifolds.GroupManifold, X; order=5) +function Jr( + M::Manifolds.GroupManifold, + X; + order=5 +) adx = ad(M, X) mapreduce(+, 0:order) do i (-adx)^i / factorial(i + 1) end end -# "The left trivialized Jacobian is referred to as the right Jacobian in much of the key literature." [Ge, van Goor, Mahony, 2024] -# argument d is a Lie algebra element giving the direction of transport -function Jl_trivialized( - M::Manifolds.GroupManifold, - d -) - adM = ad(M,d) - ead = exp(-adM) - adM \ (LinearAlgebra.I - ead) -end +# # "The left trivialized Jacobian is referred to as the right Jacobian in much of the key literature." [Ge, van Goor, Mahony, 2024] +# # argument d is a Lie algebra element giving the direction of transport +# function Jr_alt( +# M::Manifolds.GroupManifold, +# d +# ) +# adM = ad(M,d) +# ead = exp(-adM) +# TODO confirm left or right inverse here? +# adM \ (LinearAlgebra.I - ead) +# end """ @@ -54,16 +116,10 @@ Inputs: - X is a tangent vector which is to be transported - d is a tangent vector from a starting point in the direction and distance to transport -Sidenote on why lowercase tangent vector `d` (as driven by Manifolds.jl) -``` -so(2) -X = Xc*e1 = Xc*î -î = ∂/∂x = [0 -1; 0 1] # orthogonal basis -``` Notes - Default is transport without curvature estimate provided by upstream Manifolds.jl -See also: [`Jl_trivialized'](@ref), [`Jr`](@ref), `Manifolds.parallel_transport_direction`, `Manifolds.parallel_transport_to`, `Manifolds.parallel_transport_along` +See also: [`parallel_transport_curvature_2nd_lie'](@ref), [`Jr`](@ref), `Manifolds.parallel_transport_direction`, `Manifolds.parallel_transport_to`, `Manifolds.parallel_transport_along` """ parallel_transport_best( M::AbstractManifold, @@ -77,12 +133,42 @@ parallel_transport_best( ## ================================= Lie (a/A)djoints ================================= ## ---------------------------------- Almost generic ---------------------------------- + +_makeaffine(::AbstractManifold, X) = X +_makeaffine(M::Union{typeof(SpecialEuclidean(2)),typeof(SpecialEuclidean(3))}, X::ArrayPartition) = screw_matrix(M,X) + # assumes inputs are Lie algebra tangent vectors represented in matrix form ad( M::LieGroupManifoldsPirate, - A::AbstractMatrix, - B::AbstractMatrix -) = Manifolds.lie_bracket(M, A, B) + X::AbstractMatrix, + d::AbstractMatrix +) = Manifolds.lie_bracket(M, X, d) + +""" + $SIGNATURES + +Construct generic adjoint matrix for Lie group manifolds. +Input `X` is a tangent vector of the Lie algebra of group manifold `M`. + +Notes +- This implementation uses the Lie bracket over affine (or screw) matrices. + - Ref [Chirikjian 2012, Vol.2, pg.30, eq.10.59b] +""" +function ad_lie( + M::LieGroupManifoldsPirate, + X::AbstractArray, +) + # + ε = identity_element(M, X) + Es = get_basis_affine(M) + Xa = _makeaffine(M,X) + hcat( + map( + (e)->vee(M, ε, Manifolds.lie_bracket(M, Xa, e)), # Lie bracket here is also the adjoint action for M + Es + )... + ) +end Ad( @@ -123,11 +209,48 @@ end # this produces the transportMatrix P_u^vee, per [Mahony, 2024] # d is a Lie algebra element giving the direction of transport parallel_transport_direction_lie( - M::typeof(SpecialOrthogonal(3)), + M::LieGroupManifoldsPirate, d::AbstractMatrix ) = exp(ad(M, -0.5*d)) +# Matrix form parallel transport with curvature correction (2nd order) +# Jl_trv: the left trivialized Jacobian [Mahony, 2024, Theorem 4.3] +# Direction d is a Lie algebra element (tangent vector) providing direction of transport +function parallel_transport_curvature_2nd_lie( + M::LieGroupManifoldsPirate, + d +) + # Lie algebra adjoint matrix + adx = ad(M, -0.5*d) + # parallel_transport_direction_lie (without curvature) + P = exp(adx) + # include 2nd order curvature correction + return P*(LinearAlgebra.I + 1/24*adx^2) +end + +# d: Lie algebra for the direction of transport +# Xc are coordinates to be transported +parallel_transport_curvature_2nd_lie( + M::LieGroupManifoldsPirate, + d::AbstractMatrix, + Xc::AbstractVector +) = parallel_transport_curvature_2nd_lie(M, d)*Xc + + +function parallel_transport_direction_lie( + M::LieGroupManifoldsPirate, + d::AbstractMatrix, + X +) + hat( + M, + Identity(M), + parallel_transport_direction_lie(M, d) * vee(M, Identity(M), X) + ) +end + + # transport with 2nd order curvature approximation # left trivialized Jacobian [Mahony, 2024, Theorem 4.3] parallel_transport_along_2nd( @@ -135,7 +258,7 @@ parallel_transport_along_2nd( p, X::AbstractMatrix, d::AbstractMatrix -) = Jl_trivialized(M, d) * vee(M,p,X) +) = parallel_transport_curvature_2nd_lie(M, d) * vee(M,p,X) parallel_transport_best( @@ -143,7 +266,10 @@ parallel_transport_best( p, X::AbstractMatrix, d::AbstractMatrix -) = parallel_transport_along_2nd(M,p,X,d) +) = parallel_transport_along_2nd(M,p,X,d) # TODO default to Manifolds.parallel_transport_along, WIP + + + ## ----------------------------- SpecialOrthogonal(3) ----------------------------- @@ -164,42 +290,6 @@ Ad( ## For SO(3) Jr and Jl + inv closed forms, see [Chirikjian 2012, Vol2, Vol2 p.40] !!! -# Matrix form parallel transport with curvature correction (2nd order) -# left trivialized Jacobian [Mahony, 2024, Theorem 4.3] -# U is direction in which to transport (w/ 2nd order along curvature correction), -# Direction d is a Lie algebra element (tangent vector) providing direction of transport -function Jl_trivialized( - M::typeof(SpecialOrthogonal(3)), - d -) - aduv = ad(M, -0.5*d) - P = exp(aduv) - return P*(LinearAlgebra.I + 1/24*aduv^2) -end - -# d: Lie algebra for the direction of transport -# Xc are coordinates to be transported -Jl_trivialized( - M::typeof(SpecialOrthogonal(3)), - d::AbstractMatrix, - Xc::AbstractVector -) = Jl_trivialized(M, d)*Xc - - - -function parallel_transport_direction_lie( - M::typeof(SpecialOrthogonal(3)), - d::AbstractMatrix, - X -) - hat( - M, - Identity(M), - parallel_transport_direction_lie(M, d) * vee(M, Identity(M), X) - ) -end - - ## ----------------------------- SpecialEuclidean(.) ----------------------------- @@ -208,7 +298,7 @@ end # d is a Lie algebra element (tangent vector) providing the direction of transport # X is the tangent vector to be transported function ad( - M::Union{typeof(SpecialEuclidean(2)), typeof(SpecialEuclidean(3))}, + M::typeof(SpecialEuclidean(3)), d::ArrayPartition, X::ArrayPartition ) @@ -232,7 +322,7 @@ function ad( ::typeof(SpecialEuclidean(2)), d::ArrayPartition, ) - Vx = SA[d.x[2]; -d.x[1]] + Vx = SA[d.x[1][2]; -d.x[1][1]] Ω = d.x[2] vcat( hcat(Ω, Vx), diff --git a/test/testLieFundamentals.jl b/test/testLieFundamentals.jl index 9c5b957..15ec92a 100644 --- a/test/testLieFundamentals.jl +++ b/test/testLieFundamentals.jl @@ -15,18 +15,57 @@ using LinearAlgebra # @test det(Jl) == det(Jr) == 2*(1-cos||x||)/(||x||^2) +@testset "SO(2) Vector transport (w curvature) via Jacobians and Lie adjoints" begin +## + +M = SpecialOrthogonal(2) +p = Identity(M) +Xc = 0.5*randn(1) +X = hat(M, p, Xc) # random algebra element +d̂ = 0.5*randn(1) +d = hat(M, p, d̂) # direction in algebra + + +ApproxManifoldProducts.ad_lie(M, X) +# @test isapprox( +# ApproxManifoldProducts.ad_lie(M, X), +# ApproxManifoldProducts.ad(M, X) +# ) + +# ptcMat = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, d) + +@error "Missing SO(2) vector transport numerical verify tests" +# # # @test isapprox(Jl*inv(Jr), R) == [Ad(R)] +# # Jr = ptcMat +# # Jl = ptcMat' +# # @test isapprox( +# # Jl*inv(Jr), +# # ApproxManifoldProducts.Ad(M,exp_lie(M,d)); +# # atol=1e-5 +# # ) + + +## +end + + @testset "SO(3) Vector transport (w curvature) via Jacobians and Lie adjoints" begin ## M = SpecialOrthogonal(3) p = Identity(M) Xc = [1.,0,0] -X = hat(M, p, Xc) # random algebra element +X = hat(M, p, Xc) # random algebra element d̂ = [0,0,1.] -d = hat(M, p, d̂) # direction in algebra +d = hat(M, p, d̂) # direction in algebra ## piggy back utility tests used in construction of transports +@test isapprox( + ApproxManifoldProducts.ad_lie(M, X), + ApproxManifoldProducts.ad(M, X) +) + # Ad_vee and ad_vee are matrices which use multiplication to operate Ad and ad respectively # @test Ad_vee(exp_G(-0.5*coord_u)) == exp(-0.5*ad_vee(coord_u)) @test isapprox( @@ -37,8 +76,8 @@ d = hat(M, p, d̂) # direction in algebra ## parallel transport without curvature correction Y = parallel_transport_direction(M, p, X, d) # compare to Lie ad and Ad and P, bottom [Mahony 2024 p3] -Jl_trv_P = ApproxManifoldProducts.parallel_transport_direction_lie(M, d) -Yc_ = Jl_trv_P * Xc +ptcMat_ = ApproxManifoldProducts.parallel_transport_direction_lie(M, d) +Yc_ = ptcMat_ * Xc Y_ = hat(M, p, Yc_) @test isapprox( @@ -51,8 +90,8 @@ Y_ = hat(M, p, Yc_) ) # @test isapprox(Jl*inv(Jr), R) == [Ad(R)] -Jr = Jl_trv_P -Jl = Jl_trv_P' +Jr = ptcMat_ +Jl = ptcMat_' @test isapprox( Jl*inv(Jr), ApproxManifoldProducts.Ad(M,exp_lie(M,d)); @@ -74,16 +113,16 @@ Jl = Jl_trv_P' # compute transported coordinates (with Mahony 2nd order curvature correction) # is this also a push-forward -Jl_trv = ApproxManifoldProducts.Jl_trivialized(M, d) -_Y_ = Jl_trv*Xc -# _Y_ = Jl_trivialized(M, d, Xc) +ptcMat = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, d) +_Y_ = ptcMat*Xc +# _Y_ = parallel_transport_curvature_2nd_lie(M, d, Xc) # Approx check for approx curvature correctin _Y_ vs. transport in direction (wo curvature corr) Y @test isapprox(_Y_, vee(M, p, Y); atol=1e-1) # @test isapprox(Jl*inv(Jr), R) == [Ad(R)] -Jr = Jl_trv -Jl = Jl_trv' +Jr = ptcMat +Jl = ptcMat' @test isapprox( Jl*inv(Jr), ApproxManifoldProducts.Ad(M,exp_lie(M,d)); @@ -131,6 +170,38 @@ Jl = Jl_trv' end +@testset "SE(2) Vector transport (w curvature) via Jacobians and Lie adjoints" begin +## + +M = SpecialEuclidean(2) +p = Identity(M) +Xc = 0.5*randn(3) +X = hat(M, p, Xc) # random algebra element +d̂ = 0.5*randn(3) +d = hat(M, p, d̂) # direction in algebra + +@test isapprox( + ApproxManifoldProducts.ad_lie(M, X), + ApproxManifoldProducts.ad(M, X) +) + +ptcMat = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, d) + +@error "Missing SE(2) vector transport numerical verify tests" +# # @test isapprox(Jl*inv(Jr), R) == [Ad(R)] +# Jr = ptcMat +# Jl = ptcMat' +# @test isapprox( +# Jl*inv(Jr), +# ApproxManifoldProducts.Ad(M,exp_lie(M,d)); +# atol=1e-5 +# ) + + +## +end + + @testset "SE(3) Vector transport (w curvature) via Jacobians and Lie adjoints" begin ## @@ -142,12 +213,17 @@ X = hat(M, p, Xc) # random algebra element d̂ = 0.5*randn(6) d = hat(M, p, d̂) # direction in algebra -Jl_trv = ApproxManifoldProducts.Jl_trivialized(M, d) +@test isapprox( + ApproxManifoldProducts.ad_lie(M, X), + ApproxManifoldProducts.ad(M, X) +) + +ptcMat = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, d) @error "Missing SE(3) vector transport numerical verify tests" # # @test isapprox(Jl*inv(Jr), R) == [Ad(R)] -# Jr = Jl_trv -# Jl = Jl_trv' +# Jr = ptcMat +# Jl = ptcMat' # @test isapprox( # Jl*inv(Jr), # ApproxManifoldProducts.Ad(M,exp_lie(M,d)); From 6f224dbdf99d775213e74bac853b8277aa4c6b68 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 16 Jul 2024 03:13:33 -0700 Subject: [PATCH 23/31] relax Manellic construct tests slightly --- test/manellic/testManellicTree.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/manellic/testManellicTree.jl b/test/manellic/testManellicTree.jl index fbaed25..6897c85 100755 --- a/test/manellic/testManellicTree.jl +++ b/test/manellic/testManellicTree.jl @@ -978,14 +978,14 @@ end @show best_cov = abs.(Optim.minimizer(res)) -@test isapprox([0.75; 0.75; 0.75], best_cov[1:3]; atol=0.4) -@test isapprox([0.06; 0.06; 0.06], best_cov[4:6]; atol=0.04) +@test isapprox([0.75; 0.75; 0.75], best_cov[1:3]; atol=0.55) +@test isapprox([0.06; 0.06; 0.06], best_cov[4:6]; atol=0.055) mkd = ApproxManifoldProducts.manikde!_manellic(M,pts) -@test isapprox([0.75 0 0; 0 0.75 0; 0 0 0.75], getBW(mkd)[1][1:3,1:3]; atol=0.5) -@test isapprox([0.07 0 0; 0 0.07 0; 0 0 0.07], getBW(mkd)[1][4:6,4:6]; atol=0.05) +@test isapprox([0.75 0 0; 0 0.75 0; 0 0 0.75], getBW(mkd)[1][1:3,1:3]; atol=0.55) +@test isapprox([0.07 0 0; 0 0.07 0; 0 0 0.07], getBW(mkd)[1][4:6,4:6]; atol=0.055) ## end From c339150fa2daf8ddd50a694861be0f19b9052ad0 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 17 Jul 2024 03:51:24 -0700 Subject: [PATCH 24/31] Gaussian fusion with 2nd transport (Lie) --- src/CommonUtils.jl | 121 +++++++++++++++++++---------- src/services/ManifoldsOverloads.jl | 13 ++++ test/testLieFundamentals.jl | 85 +++++++++++++++++++- 3 files changed, 172 insertions(+), 47 deletions(-) diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index 32d0233..d4e98f1 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -103,8 +103,33 @@ end # return var(mkd.manifold, getPoints(mkd); kwargs...) # end +_makevec(w::AbstractVector) = w +_makevec(w::Tuple) = [w...] +function calcProductGaussians_flat( + M::AbstractManifold, + μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P (actually on-manifold) + Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; + μ0 = mean(M, _makevec(μ_)), # Tangent space reference around the evenly weighted mean of incoming points + Λ_ = inv.(Σ_), # TODO these probably need to be transported to common tangent space `μ0` -- FYI @Affie 24Q2 + weight::Real = 1.0, + do_transport_correction::Bool = true +) where {N,P<:AbstractArray,S<:AbstractMatrix{<:Real}} + # calc sum of covariances + Λ = +(Λ_...) + + # calc the covariance weighted delta means of incoming points and covariances + ΛΔμc = mapreduce(+, zip(Λ_, μ_)) do (s,u) + Δuvee = vee(M, μ0, log(M, μ0, u)) + s*Δuvee + end + + # calculate the delta mean + Δμc = Λ \ ΛΔμc + + return Δμc, inv(Λ) +end """ $SIGNATURES @@ -126,35 +151,38 @@ DevNotes: function calcProductGaussians( M::AbstractManifold, μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P (actually on-manifold) - Σ_::Union{Nothing,<:AbstractVector{S},<:NTuple{N,S}}; - dim::Integer=manifold_dimension(M), - Λ_ = inv.(Σ_), # TODO these probably need to be transported to common tangent space `u0` -- FYI @Affie 24Q2 - weight::Real = 1.0 -) where {N,P,S<:AbstractMatrix{<:Real}} - # - # calc sum of covariances - Λ = zeros(MMatrix{dim,dim}) + Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; + μ0 = mean(M, _makevec(μ_)), # Tangent space reference around the evenly weighted mean of incoming points + Λ_ = inv.(Σ_), # TODO these probably need to be transported to common tangent space `μ0` -- FYI @Affie 24Q2 + weight::Real = 1.0, + do_transport_correction::Bool = true +) where {N,P<:AbstractArray,S<:AbstractMatrix{<:Real}} + # step 1, basic/naive Gaussian product (ignoring disjointed covariance coordinates) + Δμn, Σn = calcProductGaussians_flat(M, μ_, Σ_; μ0, Λ_, weight) + Δμ = exp(M, μ0, hat(M, μ0, Δμn)) + + # for development and testing cases return without doing transport + do_transport_correction ? nothing : (return Δμ, Σn) + # FIXME first transport (push forward) covariances to common coordinates # see [Ge, van Goor, Mahony, 2024] - Λ .= sum(Λ_) + iΔμ = inv(M, Δμ) + μi = map(u->vee(M,μ0,log(M,μ0,compose(M,iΔμ,u))), μ_) + Ji = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie.(Ref(M), μi) + iJi = inv(Ji) + Σi_hat = map(S->iJi*S*(iJi'), Σ_) + + # Reset step to absorb extended μ+ coordinates into kernel on-manifold μ + # consider using Δμ in place of μ0 + Δμplusc, Σdiam = calcProductGaussians_flat(M, μi, Σi_hat; μ0, weight) + Δμplus = exp(M, μ0, hat(M, μ0, Δμplusc)) + μ_plus = compose(M,Δμ,Δμplus) + Jμ = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, Δμplus) + # iJμ = inv(Jμ) + Σ_plus = Jμ*Σdiam*(Jμ') - # naive mean - # Tangent space reference around the evenly weighted mean of incoming points - u0 = mean(M, μ_) - - # calc the covariance weighted delta means of incoming points and covariances - ΛΔμ = zeros(MVector{dim}) - for (s,u) in zip(Λ_, μ_) - # require vee as per Pennec, Caesar Ref [3.6] - Δuvee = vee(M, u0, log(M, u0, u)) - ΛΔμ += s*Δuvee - end - Λ - # calculate the delta mean - Δμ = Λ \ ΛΔμ - # return new mean and covariance - return exp(M, u0, hat(M, u0, Δμ)), inv(Λ) + return μ_plus, Σ_plus end # additional support case where covariances are passed as diagonal-only vectors @@ -165,17 +193,18 @@ calcProductGaussians( Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; dim::Integer=manifold_dimension(M), Λ_ = map(s->diagm( 1.0 ./ s), Σ_), - weight::Real = 1.0 -) where {N,P,S<:AbstractVector} = calcProductGaussians(M, μ_, nothing; dim, Λ_=Λ_ ) + weight::Real = 1.0, + do_transport_correction::Bool = true +) where {N,P,S<:AbstractVector} = calcProductGaussians(M, μ_, nothing; dim, Λ_, do_transport_correction ) # -calcProductGaussians( - M::AbstractManifold, - μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}; - dim::Integer=manifold_dimension(M), - Λ_ = diagm.( (1.0 ./ μ_) ), - weight::Real = 1.0, -) where {N,P} = calcProductGaussians(M, μ_, nothing; dim, Λ_=Λ_ ) +# # FIXME, review `./μ_`, what is this? nan risk? +# calcProductGaussians( +# M::AbstractManifold, +# μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}; +# Λ_ = diagm.( (1.0 ./ μ_) ), +# weight::Real = 1.0, +# ) where {N,P} = calcProductGaussians(M, μ_, nothing; Λ_ ) """ @@ -191,17 +220,23 @@ DevNotes """ function calcProductGaussians( M::AbstractManifold, - comps::AbstractVector{<:MvNormalKernel}; - weight::Real = 1.0 -) + kernels::Union{<:AbstractVector{K},NTuple{N,K}}; + μ0 = nothing, + weight::Real = 1.0, + do_transport_correction::Bool = true +) where {N,K <: MvNormalKernel} # CHECK this should be on-manifold for points - μ_ = mean.(comps) # This is a ArrayPartition which IS DEFINITELY ON MANIFOLD (we dispatch on mean) - Σ_ = cov.(comps) # on tangent - + μ_ = mean.(kernels) # This is a ArrayPartition which IS DEFINITELY ON MANIFOLD (we dispatch on mean) + Σ_ = cov.(kernels) # on tangent + # FIXME is parallel transport needed here for covariances from different tangent spaces? - - _μ, _Σ = calcProductGaussians(M, μ_, Σ_) - + + _μ, _Σ = if isnothing(μ0) + calcProductGaussians(M, μ_, Σ_; do_transport_correction) + else + calcProductGaussians(M, μ_, Σ_; μ0, do_transport_correction) + end + return MvNormalKernel(_μ, _Σ, weight) end diff --git a/src/services/ManifoldsOverloads.jl b/src/services/ManifoldsOverloads.jl index 8c0c124..7100555 100644 --- a/src/services/ManifoldsOverloads.jl +++ b/src/services/ManifoldsOverloads.jl @@ -3,6 +3,12 @@ const _UPSTREAM_MANIFOLDS_ADJOINT_ACTION = false # local union definition during development -- TODO consolidate upstream LieGroupManifoldsPirate = Union{ + typeof(TranslationGroup(1)), + typeof(TranslationGroup(2)), + typeof(TranslationGroup(3)), + typeof(TranslationGroup(4)), + typeof(TranslationGroup(5)), + typeof(TranslationGroup(6)), typeof(SpecialOrthogonal(2)), typeof(SpecialOrthogonal(3)), typeof(SpecialEuclidean(2)), @@ -170,6 +176,13 @@ function ad_lie( ) end +# basic fallback +# X is tangent vector (Lie algebra element) +ad( + M::LieGroupManifoldsPirate, + X +) = ad_lie(M,X) + Ad( M::Union{typeof(SpecialOrthogonal(2)), typeof(SpecialOrthogonal(3))}, diff --git a/test/testLieFundamentals.jl b/test/testLieFundamentals.jl index 15ec92a..d81c1a5 100644 --- a/test/testLieFundamentals.jl +++ b/test/testLieFundamentals.jl @@ -2,7 +2,9 @@ using Test using Manifolds using ApproxManifoldProducts +using StaticArrays using LinearAlgebra +using Distributions ## @@ -27,10 +29,10 @@ d = hat(M, p, d̂) # direction in algebra ApproxManifoldProducts.ad_lie(M, X) -# @test isapprox( -# ApproxManifoldProducts.ad_lie(M, X), -# ApproxManifoldProducts.ad(M, X) -# ) +@test isapprox( + ApproxManifoldProducts.ad_lie(M, X), + ApproxManifoldProducts.ad(M, X) +) # ptcMat = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, d) @@ -234,4 +236,79 @@ ptcMat = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, d) ## end + +@testset "(Lie Group) on-manifold Gaussian product SO(3), [Ge, van Goor, Mahony, 2024]" begin +## + +γ = 1 +ξ = 1 + +M = SpecialOrthogonal(3) +ε = Identity(M) + +X1c = γ/sqrt(3) .* SA[1; 1; -1.0] +X1 = hat(M, ε, X1c) # random algebra element +w_R1 = exp(M, ε, X1) +Σ1 = ξ .* SA[1 0 0; 0 0.75 0; 0 0 0.5] + +X2c = γ/sqrt(2) .* SA[1; -1; 0.0] +X2 = hat(M, ε, X2c) +w_R2 = exp(M, ε, X2) +Σ2 = ξ .* SA[0.5 0 0; 0 1 0; 0 0 0.75] + +# + +p1 = ApproxManifoldProducts.MvNormalKernel(; + μ = w_R1, + p = MvNormal(SA[0; 0; 0.0], Σ1) +) + +p2 = ApproxManifoldProducts.MvNormalKernel(; + μ = w_R2, + p = MvNormal(SA[0; 0; 0.0], Σ2) +) + + +# Naive product (standard linear product of Gaussians) -- reference implementation around group identity +Xcs = (X1c,X2c) +_Σs = map(s->inv(cov(s)), (p1,p2)) +_Σn = +(_Σs...) +_Σnμn = mapreduce(+, zip(_Σs, Xcs)) do (s,c) + s*c +end +μn = _Σn\_Σnμn +Σn = inv(_Σn) + +# verify calcProductGaussians utility function +p̂ = calcProductGaussians( + M, + (p1,p2); + μ0 = ε +) + +@test isapprox( + μn, + vee(M, ε, log(M, ε, mean(p̂))) +) + +# approx match for even-mean-mean rather than naive-identity-mean +p̂ = calcProductGaussians( + M, + (p1,p2); + # μ0 = ε +) +@test isapprox( + μn, + vee(M, ε, log(M, ε, mean(p̂))); + atol=1e-1 # NOTE looser bound for even-mean-mean case vs naive-identity-mean case +) + +## + + + +## +end + + # \ No newline at end of file From dd714d8197bda380b03e6bf0a634b86689132cc3 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 17 Jul 2024 03:59:27 -0700 Subject: [PATCH 25/31] wip calcProdGau with transport --- src/CommonUtils.jl | 10 ++++++---- test/testLieFundamentals.jl | 12 ++++++++++-- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index d4e98f1..9964da8 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -167,7 +167,8 @@ function calcProductGaussians( # FIXME first transport (push forward) covariances to common coordinates # see [Ge, van Goor, Mahony, 2024] iΔμ = inv(M, Δμ) - μi = map(u->vee(M,μ0,log(M,μ0,compose(M,iΔμ,u))), μ_) + μi_̂ = map(u->log(M,μ0,Manifolds.compose(M,iΔμ,u)), μ_) + μi = map(u->vee(M,μ0,u), μi_̂ ) Ji = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie.(Ref(M), μi) iJi = inv(Ji) Σi_hat = map(S->iJi*S*(iJi'), Σ_) @@ -175,9 +176,10 @@ function calcProductGaussians( # Reset step to absorb extended μ+ coordinates into kernel on-manifold μ # consider using Δμ in place of μ0 Δμplusc, Σdiam = calcProductGaussians_flat(M, μi, Σi_hat; μ0, weight) - Δμplus = exp(M, μ0, hat(M, μ0, Δμplusc)) - μ_plus = compose(M,Δμ,Δμplus) - Jμ = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, Δμplus) + Δμplus_̂ = hat(M, μ0, Δμplusc) + Δμplus = exp(M, μ0, Δμplus_̂ ) + μ_plus = Manifolds.compose(M,Δμ,Δμplus) + Jμ = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, Δμplus_̂ ) # iJμ = inv(Jμ) Σ_plus = Jμ*Σdiam*(Jμ') diff --git a/test/testLieFundamentals.jl b/test/testLieFundamentals.jl index d81c1a5..411f9f5 100644 --- a/test/testLieFundamentals.jl +++ b/test/testLieFundamentals.jl @@ -283,7 +283,8 @@ end p̂ = calcProductGaussians( M, (p1,p2); - μ0 = ε + μ0 = ε, + do_transport_correction = false ) @test isapprox( @@ -295,7 +296,8 @@ p̂ = calcProductGaussians( p̂ = calcProductGaussians( M, (p1,p2); - # μ0 = ε + # μ0 = ε, + do_transport_correction = false ) @test isapprox( μn, @@ -305,6 +307,12 @@ p̂ = calcProductGaussians( ## +p̂ = calcProductGaussians( + M, + (p1,p2); + # μ0 = ε, + do_transport_correction = true +) ## From 1628c94905799410551adebbc6c3dadf852b2bc3 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 29 Jul 2024 02:27:37 -0700 Subject: [PATCH 26/31] fix calcProdGau w correction --- src/CommonUtils.jl | 13 +++++++------ test/testLieFundamentals.jl | 3 +++ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index 9964da8..54a00ee 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -167,15 +167,16 @@ function calcProductGaussians( # FIXME first transport (push forward) covariances to common coordinates # see [Ge, van Goor, Mahony, 2024] iΔμ = inv(M, Δμ) - μi_̂ = map(u->log(M,μ0,Manifolds.compose(M,iΔμ,u)), μ_) - μi = map(u->vee(M,μ0,u), μi_̂ ) - Ji = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie.(Ref(M), μi) - iJi = inv(Ji) - Σi_hat = map(S->iJi*S*(iJi'), Σ_) + μi_ = map(u->Manifolds.compose(M,iΔμ,u), μ_) + μi_̂ = map(u->log(M,μ0,u), μi_) + # μi = map(u->vee(M,μ0,u), μi_̂ ) + Ji = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie.(Ref(M), μi_̂ ) + iJi = inv.(Ji) + Σi_hat = map((J,S)->J*S*(J'), iJi, Σ_) # Reset step to absorb extended μ+ coordinates into kernel on-manifold μ # consider using Δμ in place of μ0 - Δμplusc, Σdiam = calcProductGaussians_flat(M, μi, Σi_hat; μ0, weight) + Δμplusc, Σdiam = ApproxManifoldProducts.calcProductGaussians_flat(M, μi_, Σi_hat; μ0, weight) Δμplus_̂ = hat(M, μ0, Δμplusc) Δμplus = exp(M, μ0, Δμplus_̂ ) μ_plus = Manifolds.compose(M,Δμ,Δμplus) diff --git a/test/testLieFundamentals.jl b/test/testLieFundamentals.jl index 411f9f5..1d6460e 100644 --- a/test/testLieFundamentals.jl +++ b/test/testLieFundamentals.jl @@ -319,4 +319,7 @@ p̂ = calcProductGaussians( end + + + # \ No newline at end of file From 2b709f98bb241fe39eeff8b57b51f56323007659 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 29 Jul 2024 02:29:41 -0700 Subject: [PATCH 27/31] fixed flat product todo --- src/CommonUtils.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index 54a00ee..f8db7a6 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -112,7 +112,7 @@ function calcProductGaussians_flat( μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P (actually on-manifold) Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; μ0 = mean(M, _makevec(μ_)), # Tangent space reference around the evenly weighted mean of incoming points - Λ_ = inv.(Σ_), # TODO these probably need to be transported to common tangent space `μ0` -- FYI @Affie 24Q2 + Λ_ = inv.(Σ_), weight::Real = 1.0, do_transport_correction::Bool = true ) where {N,P<:AbstractArray,S<:AbstractMatrix{<:Real}} From 2155af3cab3bbe3fba6172e6df8526bd37ea881e Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 29 Jul 2024 02:45:10 -0700 Subject: [PATCH 28/31] rm extra comments, fixmes todos --- src/CommonUtils.jl | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/src/CommonUtils.jl b/src/CommonUtils.jl index f8db7a6..7846c6f 100644 --- a/src/CommonUtils.jl +++ b/src/CommonUtils.jl @@ -153,7 +153,7 @@ function calcProductGaussians( μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}, # point type commonly known as P (actually on-manifold) Σ_::Union{<:AbstractVector{S},<:NTuple{N,S}}; μ0 = mean(M, _makevec(μ_)), # Tangent space reference around the evenly weighted mean of incoming points - Λ_ = inv.(Σ_), # TODO these probably need to be transported to common tangent space `μ0` -- FYI @Affie 24Q2 + Λ_ = inv.(Σ_), weight::Real = 1.0, do_transport_correction::Bool = true ) where {N,P<:AbstractArray,S<:AbstractMatrix{<:Real}} @@ -164,7 +164,7 @@ function calcProductGaussians( # for development and testing cases return without doing transport do_transport_correction ? nothing : (return Δμ, Σn) - # FIXME first transport (push forward) covariances to common coordinates + # first transport (push forward) covariances to common coordinates # see [Ge, van Goor, Mahony, 2024] iΔμ = inv(M, Δμ) μi_ = map(u->Manifolds.compose(M,iΔμ,u), μ_) @@ -181,7 +181,6 @@ function calcProductGaussians( Δμplus = exp(M, μ0, Δμplus_̂ ) μ_plus = Manifolds.compose(M,Δμ,Δμplus) Jμ = ApproxManifoldProducts.parallel_transport_curvature_2nd_lie(M, Δμplus_̂ ) - # iJμ = inv(Jμ) Σ_plus = Jμ*Σdiam*(Jμ') # return new mean and covariance @@ -201,13 +200,6 @@ calcProductGaussians( ) where {N,P,S<:AbstractVector} = calcProductGaussians(M, μ_, nothing; dim, Λ_, do_transport_correction ) # -# # FIXME, review `./μ_`, what is this? nan risk? -# calcProductGaussians( -# M::AbstractManifold, -# μ_::Union{<:AbstractVector{P},<:NTuple{N,P}}; -# Λ_ = diagm.( (1.0 ./ μ_) ), -# weight::Real = 1.0, -# ) where {N,P} = calcProductGaussians(M, μ_, nothing; Λ_ ) """ @@ -232,8 +224,7 @@ function calcProductGaussians( μ_ = mean.(kernels) # This is a ArrayPartition which IS DEFINITELY ON MANIFOLD (we dispatch on mean) Σ_ = cov.(kernels) # on tangent - # FIXME is parallel transport needed here for covariances from different tangent spaces? - + # parallel transport needed for covariances from different tangent spaces _μ, _Σ = if isnothing(μ0) calcProductGaussians(M, μ_, Σ_; do_transport_correction) else From 2c0c0a9ec306f8c4a5ee59ec411a3c950cb744b4 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 29 Jul 2024 09:02:54 -0700 Subject: [PATCH 29/31] bit more Lie product test --- test/testLieFundamentals.jl | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/test/testLieFundamentals.jl b/test/testLieFundamentals.jl index 1d6460e..37d131a 100644 --- a/test/testLieFundamentals.jl +++ b/test/testLieFundamentals.jl @@ -314,6 +314,21 @@ p̂ = calcProductGaussians( do_transport_correction = true ) +@test isapprox( + [0;0;0.0], + mean(p̂.p); + atol=1e-10 +) + +@test isapprox( + μn, + vee(M, ε, log(M, ε, mean(p̂))); + atol=1e-1 # NOTE looser bound for even-mean-mean case vs naive-identity-mean case +) + +cov(p̂) + +p ## end From 889bc181f63ff5cb71c8d35413d596d369a71473 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 29 Jul 2024 09:03:03 -0700 Subject: [PATCH 30/31] fix --- test/testLieFundamentals.jl | 2 -- 1 file changed, 2 deletions(-) diff --git a/test/testLieFundamentals.jl b/test/testLieFundamentals.jl index 37d131a..1c24fb6 100644 --- a/test/testLieFundamentals.jl +++ b/test/testLieFundamentals.jl @@ -326,9 +326,7 @@ p̂ = calcProductGaussians( atol=1e-1 # NOTE looser bound for even-mean-mean case vs naive-identity-mean case ) -cov(p̂) -p ## end From 2ab8e23191f139a5a5c966c4546d5727ed280218 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 8 Aug 2024 02:59:14 -0700 Subject: [PATCH 31/31] bump v0.8.5 --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 3b4db3c..0195624 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.8.4" +version = "0.8.5" [deps] Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"