Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #272 from JuliaRobotics/master
release v0.8.4-rc1
- Loading branch information
Showing
13 changed files
with
313 additions
and
108 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,31 @@ | ||
|
||
|
||
|
||
# """ | ||
# $TYPEDEF | ||
|
||
# Elliptical structure for use in a (Manellic) Ball Tree. | ||
# """ | ||
# struct HyperEllipse{P <:AbstractArray,D,DD} | ||
# """ manifold point at which this ellipse is based """ | ||
# point::P | ||
# """ Covariance of coords at either TBD this point or some other reference point? """ | ||
# coord_cov::SMatrix{D,D,Float64,DD} | ||
# end | ||
|
||
# ManellicTree | ||
|
||
# Short for Manifold Ellipse Metric Tree | ||
# starting as a balanced tree, relax to unbalanced in future. | ||
struct ManellicTree{M,D<:AbstractVector,N,HL,HT} | ||
manifold::M | ||
data::D | ||
weights::MVector{N,<:Real} | ||
permute::MVector{N,Int} | ||
# kernels::ArrayPartition{<:Number,KT} | ||
leaf_kernels::SizedVector{N,HL} | ||
tree_kernels::SizedVector{N,HT} | ||
left_idx::MVector{N,Int} | ||
right_idx::MVector{N,Int} | ||
end | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,57 +1,82 @@ | ||
|
||
# also makes static | ||
function projectSymPosDef(c::AbstractMatrix) | ||
s = size(c) | ||
# pretty fast to make or remake isbitstype form matrix | ||
_c = SMatrix{s...}(c) | ||
issymmetric(_c) ? _c : project(SymmetricPositiveDefinite(s[1]),_c,_c) | ||
end | ||
|
||
|
||
abstract type AbstractKernel end | ||
|
||
@kwdef struct MvNormalKernel{T,M} <: AbstractKernel | ||
@kwdef struct MvNormalKernel{T,M,iM} <: AbstractKernel | ||
p::MvNormal{T,M} | ||
sqrt_iΣ::M = sqrt(inv(p.Σ)) | ||
# 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) | ||
_c = projectSymPosDef(c) | ||
p=MvNormal(m,_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Σ) | ||
end | ||
|
||
Statistics.mean(m::MvNormalKernel) = m.p.μ | ||
Statistics.cov(m::MvNormalKernel) = m.p.Σ # note also about m.sqrt_iΣ | ||
|
||
|
||
function distanceMalahanobisCoordinates( | ||
M::AbstractManifold, | ||
K::AbstractKernel, | ||
q, | ||
basis=DefaultOrthonormalBasis() | ||
basis=DefaultOrthogonalBasis() | ||
) | ||
p = mean(K) | ||
i_p = inv(M,p) | ||
pq = Manifolds.compose(M, i_p, q) | ||
X = log(M, p, pq) | ||
Xc = get_coordinates(M, p, X, basis) | ||
ϵ = identity_element(M,q) | ||
X = log(M, ϵ, pq) | ||
Xc = get_coordinates(M, ϵ, X, basis) | ||
return K.sqrt_iΣ*Xc | ||
end | ||
|
||
function distanceMalahanobisSq( | ||
M::AbstractManifold, | ||
K::AbstractKernel, | ||
q, | ||
basis=DefaultOrthonormalBasis() | ||
basis=DefaultOrthogonalBasis() | ||
) | ||
δc = distanceMalahanobisCoordinates(M,K,q,basis) | ||
p = mean(K) | ||
# ϵ = identity_element(M, q) | ||
X = get_vector(M, p, δc, basis) | ||
ϵ = identity_element(M, q) | ||
X = get_vector(M, ϵ, δc, basis) | ||
return inner(M, p, X, X) | ||
end | ||
|
||
# function distance( | ||
# M::AbstractManifold, | ||
# p::AbstractVector, | ||
# q::AbstractVector, | ||
# kernel=(_p) -> MvNormalKernel( | ||
# p=MvNormal(_p,SVector(ones(manifold_dimension(M))...)) | ||
# ), | ||
# distFnc::Function=distanceMalahanobisSq | ||
# ) | ||
# distFnc(M, kernel(p), q) | ||
# end | ||
|
||
function _distance( | ||
M::AbstractManifold, | ||
p::AbstractVector, | ||
q::AbstractVector, | ||
kernel = (_p) -> MvNormalKernel( | ||
p=MvNormal(_p,SVector(ntuple((s)->1,manifold_dimension(M))...)) | ||
), | ||
distFnc::Function=distanceMalahanobisSq, | ||
# distFnc::Function=distanceMalahanobisSq, | ||
) | ||
distFnc(M, kernel(p), q) | ||
end | ||
|
||
|
||
""" | ||
$SIGNATURES | ||
$SIGNATURES | ||
Normal kernel used for Hilbert space embeddings. | ||
""" | ||
ker(M::AbstractManifold, p, q, sigma::Real=0.001) = exp( -sigma*(distance(M, p, q)^2) ) | ||
ker(M::AbstractManifold, p, q, sigma::Real=0.001, distFnc=(_M,_p,_q)->distance(_M,_p,_q)^2) = exp( -sigma*distFnc(M, p, q) ) # _distance(M,p,q) # | ||
|
Oops, something went wrong.