Merge pull request #126 from JuliaRobotics/master

NEWS.md

@@ -1,5 +1,9 @@
 Major news relating to breaking changes in ApproxManifoldProducts.jl
 
+## v0.4
+
+- `ManifoldKernelDensity` is the primary density approximation method.
+- `rand(::MKD,N)` now returns a `::Vector{P}` of points type `P`, not a matrix of coordinate columns.
 ## v0.3 
 
 - Upgrade to ManifoldsBase.jl v0.11 with `AbstractManifold`.

Project.toml

@@ -1,7 +1,7 @@
 name = "ApproxManifoldProducts"
 uuid = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 keywords = ["MM-iSAMv2", "SLAM", "inference", "sum-product", "belief-propagation", "nonparametric", "manifolds", "functional"]
-version = "0.4.9"
+version = "0.4.10"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

src/API.jl

@@ -4,11 +4,6 @@ export productbelief
 
 
 
-# TODO move to better src file location
-isPartial(mkd::ManifoldKernelDensity{M,B,L}) where {M,B,L} = true
-isPartial(mkd::ManifoldKernelDensity{M,B,Nothing}) where {M,B} = false
-
-
 """
     $SIGNATURES
 
@@ -59,23 +54,24 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity},
     # @show Ndim(ff[1]), Npts(ff[1]), getPoints(ff[1],false)[1]
     return (makeCopy ? x->deepcopy(x) : x->x)(ff[1])
   end
+
 
   glbs = KDE.makeEmptyGbGlb();
   glbs.recordChoosen = recordLabels
 
   # TODO DEPRECATE ::NTuple{Symbol} approach
   manif = convert(Tuple, mani)  #[partialDimsWorkaround]
   addopT, diffopT, getManiMu, _ = buildHybridManifoldCallbacks(manif)
-
-
+  
+  
   bws = 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...
   # @cast oldpts_[i,j] := oldPoints[j][i]
   # oldpts = collect(oldpts_)
   inplace = kde!(oldpts, bws, addopT, diffopT ); # rand(ndims,N)
-
+  
   # TODO REMOVE
   _ff = (x->x.belief).(ff)
   partialDimMask = Vector{BitVector}(undef, length(ff))
@@ -89,6 +85,8 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity},
       end
     end
   end
+
+  # @assert !isPartial(ff[1]) "currently only supports first product element as full density, isPartial.=$(isPartial.(ff)), dims.=$(Ndim.(_ff))"
 
   ## TODO check both _ff and inplace use a matrix of coordinates (columns)
   # expects Matrix with columns as samples and rows are coordinate dimensions
@@ -112,7 +110,7 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity},
     for i in 1:N
       selectedLabels[i] = Int[]
       for j in 1:length(ff)
-        push!(selectedLabels[i], lc[i][j][nLevels] - Npts(ff[j]))
+        push!(selectedLabels[i], lc[i][j][nLevels])
       end
     end
   end

src/ApproxManifoldProducts.jl

@@ -16,16 +16,16 @@ using NLsolve
 import Optim
 using CoordinateTransformations
 using Requires
-using SLEEFPirates
+# using SLEEFPirates
 using LinearAlgebra
-# using JSON2
 using TensorCast
 using StaticArrays
 using Logging
+using Statistics
 
 import Base: *, isapprox, convert
-# import KernelDensityEstimate: kde!
 import LinearAlgebra: rotate!
+import Statistics: mean
 
 
 const AMP = ApproxManifoldProducts
@@ -56,7 +56,10 @@ export
   getKDEManifoldBandwidths,
   manifoldProduct,
   manikde!,
-  calcVariableCovarianceBasic
+  calcCovarianceBasic,
+  isPartial,
+  mean,
+  calcProductGaussians
 
 
 # internal features not exported

src/CommonUtils.jl

@@ -61,20 +61,81 @@ function updateProductSample( dest::BallTreeDensity,
 end
 
 # Returns the covariance (square), not deviation
-function calcVariableCovarianceBasic(M::AbstractManifold, ptsArr::Vector{P}) where P
+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 "calcVariableCovarianceBasic" μ
-  @debug "calcVariableCovarianceBasic" Σ
+  @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

src/Deprecated.jl

@@ -1,5 +1,19 @@
 
 
+## ======================================================================================================
+## Remove below before v0.6
+## ======================================================================================================
+
+
+# function calcMean(mkd::ManifoldKernelDensity{M}) where {M <: ManifoldsBase.AbstractManifold}
+#   data = getPoints(mkd)
+#   # Returns the mean point on manifold for consitency
+#   mean(mkd.manifold, data)  
+# end
+
+
+@deprecate calcVariableCovarianceBasic(M::AbstractManifold, vecP::Vector{P}) where P calcCovarianceBasic(M, vecP)
+
 ## ======================================================================================================
 ## Remove below before v0.5
 ## ======================================================================================================

src/services/ManifoldKernelDensity.jl

@@ -6,6 +6,25 @@ export getKDERange, getKDEMax, getKDEMean, getKDEfit
 export sample, rand, resample, kld, minkld
 export calcMean
 
+
+function Statistics.mean(mkd::ManifoldKernelDensity, aspartial::Bool=true)
+  M = if aspartial && isPartial(mkd)
+    getManifoldPartial(mkd.manifold, mkd._partial)
+  else
+    mkd.manifold
+  end
+
+  mean(mkd.manifold, getPoints(mkd, aspartial))
+end
+
+"""
+    $SIGNATURES
+
+Alias for overloaded `Statistics.mean`.
+"""
+calcMean(mkd::ManifoldKernelDensity, aspartial::Bool=true) = mean(mkd, aspartial)
+
+
 function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P}
   printstyled(io, "ManifoldKernelDensity{", bold=true, color=:blue )
   println(io)
@@ -23,12 +42,17 @@ function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P}
   println(io)
   println(io, " }(")
   println(io, "  Npts:  ", Npts(mkd.belief))
-  println(io, "  dims:  ", Ndim(mkd.belief))
+  print(io, "  dims:  ", Ndim(mkd.belief))
+  printstyled(io, isPartial(mkd) ? "* --> $(length(mkd._partial))" : "", bold=true)
+  println(io)
   println(io, "  prtl:  ", mkd._partial)
-  println(io, "  bws:   ", getBW(mkd.belief)[:,1] .|> x->round(x,digits=4))
-  println(io, "  ipc:   ", mkd.infoPerCoord .|> x->round(x,digits=4))
+  bw = getBW(mkd.belief)[:,1]
+  pvec = isPartial(mkd) ? mkd._partial : collect(1:length(bw))
+  println(io, "  bws:   ", bw[pvec] .|> x->round(x,digits=4))
+  println(io, "  ipc:   ", mkd.infoPerCoord[pvec] .|> x->round(x,digits=4))
   try
-    mn = mean(mkd.manifold, getPoints(mkd, false))
+    # mn = mean(mkd.manifold, getPoints(mkd, false))
+    mn = mean(mkd)
     println(io, "   mean:  ", round.(mn',digits=4))
   catch
   end
@@ -67,6 +91,12 @@ function ManifoldKernelDensity( M::MB.AbstractManifold,
   manis = convert(Tuple, M)
   # find or have the bandwidth
   _bw = bw === nothing ? getKDEManifoldBandwidths(arr, manis ) : bw
+  # NOTE workaround for partials and user did not specify a bw
+  if bw === nothing && partial !== nothing
+    mask = ones(Int, length(_bw)) .== 1
+    mask[partial] .= false
+    _bw[mask] .= 1.0
+  end
   addopT, diffopT, _, _ = buildHybridManifoldCallbacks(manis)
   bel = KernelDensityEstimate.kde!(arr, _bw, addopT, diffopT)
   return ManifoldKernelDensity(M, bel, partial, u0, infoPerCoord)
@@ -97,12 +127,20 @@ function _buildManifoldPartial( fullM::MB.AbstractManifold,
   return ProductManifold(newMani...)
 end
 
+"""
+    $SIGNATURES
+
+Return true if this ManifoldKernelDensity is a partial.
+"""
+isPartial(mkd::ManifoldKernelDensity{M,B,L}) where {M,B,L} = true
+isPartial(mkd::ManifoldKernelDensity{M,B,Nothing}) where {M,B} = false
+
 # override
 function marginal(x::ManifoldKernelDensity{M,B}, 
-  dims::AbstractVector{<:Integer}  ) where {M <: AbstractManifold , B}
-#
-ldims::Vector{Int} = collect(dims)
-ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0)
+                  dims::AbstractVector{<:Integer}  ) where {M <: AbstractManifold , B}
+  #
+  ldims::Vector{Int} = collect(dims)
+  ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0)
 end
 
 function marginal(x::ManifoldKernelDensity{M,B,L}, 
@@ -180,11 +218,4 @@ Base.convert(::Type{B}, mkd::ManifoldKernelDensity{M,B}) where {M,B<:BallTreeDen
 
 
 
-function calcMean(mkd::ManifoldKernelDensity{M}) where {M <: ManifoldsBase.AbstractManifold}
-  data = getPoints(mkd)
-  # Returns the mean point on manifold for consitency
-  mean(mkd.manifold, data)  
-end
-
-
-#
+#