-
Notifications
You must be signed in to change notification settings - Fork 4
/
CommonUtils.jl
168 lines (133 loc) · 5.8 KB
/
CommonUtils.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
# Common Utils
function resid2DLinear(μ, mus, Lambdas; diffop::Function=-) # '-' exploits EuclideanManifold commutativity a-b = b-a
dμ = broadcast(diffop, μ, mus) # mus .- μ ## μ .\ mus
# @show round.(dμ, digits=4)
ret = sum( Lambdas.*dμ )
return ret
end
function solveresid2DLinear!(res, x, mus, Lambdas; diffop::Function=-)::Nothing
res[1] = resid2DLinear(x, mus, Lambdas, diffop=diffop)
nothing
end
# import ApproxManifoldProducts: resid2DLinear, solveresid2DLinear
function solveresid2DLinear(res, x, mus, Lambdas; diffop::Function=-)::Float64
solveresid2DLinear!(res, x, mus, Lambdas, diffop=diffop)
return res[1]
end
"""
$SIGNATURES
A clunky repeat calculation of one product kernel.
"""
function updateProductSample( dest::BallTreeDensity,
proposals::Vector{BallTreeDensity},
manifolds::Tuple,
smplIdx::Int,
labels::Vector{Int})
#
Ndens = length(proposals)
Ndim = Ndim(dest)
densLen = Npts.(proposals)
calclambdas = zeros(Ndim, Ndens)
calcmu = zeros(Ndim, Ndens)
destMu = zeros(Ndim)
destCov = 0.0
@inbounds @fastmath @simd for dim in 1:Ndim
for j in 1:Ndens
calclambdas[dim,j] = 1.0/getBW(proposals[j])[dim,labels[j]]
calcmu[dim,j] = getPoints(proposals[j])[dim,labels[j]]
end
destCov = getLambda(calclambdas)
destCov = 1.0/destCov
# μ = 1/Λ * Λμ ## i.e. already scaled to mean only
destMu[dim] = getMu(calcmu[dim, :], calclambdas[dim, :], destCov)
end
# previous points
pts = getPoints(dest)
pts[:,smplIdx] = destMu
manikde!(pts, manifolds)
end
# Returns the covariance (square), not deviation
function calcCovarianceBasic(M::AbstractManifold, ptsArr::Vector{P}) where P
#TODO double check the maths,. it looks like its working at least for groups
μ = mean(M, ptsArr)
Xcs = vee.(Ref(M), Ref(μ), log.(Ref(M), Ref(μ), ptsArr))
Σ = mean(Xcs .* transpose.(Xcs))
@debug "calcCovarianceBasic" μ
@debug "calcCovarianceBasic" Σ
# TODO don't know what to do here so keeping as before, #FIXME it will break
# a change between this and previous is that a full covariance matrix is returned
msst = Σ
msst_ = 0 < sum(1e-10 .< msst) ? maximum(msst) : 1.0
return msst_
end
"""
$SIGNATURES
Calculate covariance weighted mean as product of incoming Gaussian points ``μ_`` and coordinate covariances ``Σ_``.
Notes
- Return both weighted mean and new covariance (teh congruent product)
- More efficient helper function allows passing keyword inverse covariances `Λ_` instead.
- Assume `size(Σ_[1],1) == manifold_dimension(M)`.
- 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.
"""
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})
Λ .= sum(Λ_)
# 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(Λ)
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}};
dim::Integer=manifold_dimension(M),
Λ_ = diagm.( (1.0 ./ μ_) ),
) where {N,P} = calcProductGaussians(M, μ_, nothing; dim=dim, Λ_=Λ_ )
#
# """
# $SIGNATURES
# Once a Gibbs product is available, this function can be used to update the product assuming some change to the input
# to some or some or all of the input density kernels.
# Notes
# - This function does not resample a new posterior sample pairing of inputs, only updates with existing
# """
# function _updateMetricTreeDensityProduct( npd0::BallTreeDensity,
# trees::Array{BallTreeDensity,1},
# anFcns,
# anParams;
# Niter::Int=3,
# addop::Tuple=(+,),
# diffop::Tuple=(-,),
# getMu::Tuple=(getEuclidMu,),
# getLambda::T4=(getEuclidLambda,),
# glbs = makeEmptyGbGlb(),
# addEntropy::Bool=true )
# #
# end
#