Merge pull request #162 from JuliaRobotics/master

v0.4.17-rc1

Project.toml

@@ -1,7 +1,7 @@
 name = "ApproxManifoldProducts"
 uuid = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 keywords = ["MM-iSAMv2", "SLAM", "inference", "sum-product", "belief-propagation", "nonparametric", "manifolds", "functional"]
-version = "0.4.16"
+version = "0.4.17"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
@@ -34,13 +34,13 @@ UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 Unicode = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5"
 
 [compat]
-CoordinateTransformations = "0.5, 0.6, 0.8, 0.9"
+CoordinateTransformations = "0.5, 0.6"
 DocStringExtensions = "0.7, 0.8, 0.9"
-KernelDensityEstimate = "0.5.6"
+KernelDensityEstimate = "0.5.9"
 Manifolds = "0.6"
 ManifoldsBase = "0.11, 0.12"
 NLsolve = "3, 4"
-Optim = "0.16, 0.17, 0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 1"
+Optim = "1"
 Reexport = "0.2, 1.0"
 Requires = "0.5, 1"
 SLEEFPirates = "0.5, 0.6"
@@ -51,7 +51,9 @@ julia = "1.4"
 
 [extras]
 DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"
+FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
+JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["DataFrames", "Test"]
+test = ["DataFrames", "FileIO", "JLD2", "Test"]

src/API.jl

@@ -42,6 +42,8 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity},
                           addEntropy::Bool=true,
                           recordLabels::Bool=false,
                           selectedLabels::Vector{Vector{Int}}=Vector{Vector{Int}}(),
+                          _randU=Vector{Float64}(),
+                          _randN=Vector{Float64}(),
                           logger=ConsoleLogger()  ) where {M <: MB.AbstractManifold, P}
   #
   # check quick exit
@@ -87,7 +89,21 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity},
     end
   end
 
-  # @assert !isPartial(ff[1]) "currently only supports first product element as full density, isPartial.=$(isPartial.(ff)), dims.=$(Ndim.(_ff))"
+  ndims = maximum(Ndim.(_ff))
+  Ndens = length(_ff)
+  Np    = Npts(inplace)
+  maxNp = maximum(Int[Np; Npts.(_ff)])
+  Nlevels = floor(Int,(log(Float64(maxNp))/log(2.0))+1.0)
+  if 0 == length(_randU)
+    _len = Int(Np*Ndens*(Niter+2)*Nlevels)
+    resize!(_randU, _len)
+    _randU .= rand(_len)
+  end
+  if 0 == length(_randN)
+    _len = Int(ndims*Np*(Nlevels+1))
+    resize!(_randN, _len)
+    _randN .= randn(_len) 
+  end
 
   ## TODO check both _ff and inplace use a matrix of coordinates (columns)
   # expects Matrix with columns as samples and rows are coordinate dimensions
@@ -98,7 +114,14 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity},
                           diffop=diffopT,
                           getMu=getManiMu,
                           glbs=glbs,
-                          addEntropy=addEntropy  );
+                          addEntropy=addEntropy,
+                          ndims=ndims,
+                          Ndens=Ndens,
+                          Np=Np,
+                          maxNp=maxNp,
+                          Nlevels=Nlevels,
+                          randU=_randU,
+                          randN=_randN  );
   #
 
   if recordLabels

src/ApproxManifoldProducts.jl

@@ -26,6 +26,7 @@ using Statistics
 import Base: *, isapprox, convert
 import LinearAlgebra: rotate!
 import Statistics: mean
+import KernelDensityEstimate: getPoints, getBW
 
 
 const AMP = ApproxManifoldProducts

src/Legacy.jl

@@ -101,11 +101,10 @@ end
 # getManifold(x::ManifoldKernelDensity) = x.manifold
 
 
-import KernelDensityEstimate: getPoints, getBW, Ndim, Npts, getWeights, marginal 
+import KernelDensityEstimate: Ndim, Npts, getWeights, marginal 
 import KernelDensityEstimate: getKDERange, getKDEMax, getKDEMean, getKDEfit
 import KernelDensityEstimate: sample, rand, resample, kld, minkld
 
-getBW(x::ManifoldKernelDensity, w...;kw...) = getBW(x.belief,w...;kw...)
 
 Ndim(x::ManifoldKernelDensity, w...;kw...) = Ndim(x.belief,w...;kw...)
 Npts(x::ManifoldKernelDensity, w...;kw...) = Npts(x.belief,w...;kw...)

src/entities/ManifoldKernelDensity.jl

@@ -1,6 +1,6 @@
 
 
-struct ManifoldKernelDensity{M <: MB.AbstractManifold{MB.ℝ}, B <: BallTreeDensity, L, P}
+struct ManifoldKernelDensity{M <: MB.AbstractManifold, B <: BallTreeDensity, L, P}
   manifold::M
   """ legacy expects matrix of coordinates (as columns) """
   belief::B

src/services/ManifoldKernelDensity.jl

@@ -104,50 +104,6 @@ Alias for overloaded `Statistics.mean`.
 calcMean(mkd::ManifoldKernelDensity, aspartial::Bool=true) = mean(mkd, aspartial)
 
 
-
-# internal workaround function for building partial submanifold dimensions, must be upgraded/standarized
-function _buildManifoldPartial( fullM::MB.AbstractManifold, 
-                                partial_coord_dims )
-  #
-  # temporary workaround during Manifolds.jl integration
-  manif = convert(Tuple, fullM)[partial_coord_dims]
-  # 
-  newMani = MB.AbstractManifold[]
-  for me in manif
-    push!(newMani, _reducePartialManifoldElements(me))
-  end
-
-  # assume independent dimensions for definition, ONLY USED AS DECORATOR AT THIS TIME, FIXME
-  return ProductManifold(newMani...)
-end
-
-"""
-    $SIGNATURES
-
-Return true if this ManifoldKernelDensity is a partial.
-"""
-isPartial(mkd::ManifoldKernelDensity{M,B,L}) where {M,B,L} = true
-isPartial(mkd::ManifoldKernelDensity{M,B,Nothing}) where {M,B} = false
-
-# override
-function marginal(x::ManifoldKernelDensity{M,B}, 
-                  dims::AbstractVector{<:Integer}  ) where {M <: AbstractManifold , B}
-  #
-  ldims::Vector{Int} = collect(dims)
-  ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0)
-end
-
-function marginal(x::ManifoldKernelDensity{M,B,L}, 
-                  dims::AbstractVector{<:Integer}  ) where {M <: AbstractManifold , B, L <: AbstractVector{<:Integer}}
-  #
-  # @show dims x._partial
-  ldims::Vector{Int} = intersect(x._partial, dims)
-  ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0)
-end
-# manis = convert(Tuple, x.manifold)
-# partMani = _reducePartialManifoldElements(manis[dims])
-# pts = getPoints(x)
-
 _getFieldPartials(mkd::ManifoldKernelDensity{M,B,Nothing}, field::Function, aspartial::Bool=true) where {M,B} = field(mkd)
 
 function _getFieldPartials(mkd::ManifoldKernelDensity{M,B,<:AbstractVector}, field::Function, aspartial::Bool=true) where {M,B}
@@ -167,34 +123,37 @@ function _getFieldPartials(mkd::ManifoldKernelDensity{M,B,<:AbstractVector}, fie
   end
 end
 
+
+
 getInfoPerCoord(mkd::ManifoldKernelDensity, aspartial::Bool=true) = _getFieldPartials(mkd, x->x.infoPerCoord, aspartial)
 
 getBandwidth(mkd::ManifoldKernelDensity, aspartial::Bool=true) = _getFieldPartials(mkd, x->getBW(x)[:,1], aspartial)
 
 
-function antimarginal(newM::AbstractManifold,
-                      u0,
-                      mkd::ManifoldKernelDensity, 
-                      newpartial::AbstractVector{<:Integer} )
+# internal workaround function for building partial submanifold dimensions, must be upgraded/standarized
+function _buildManifoldPartial( fullM::MB.AbstractManifold, 
+                                partial_coord_dims )
   #
-
-  # convert to antimarginal by copying user provided example point for bigger manifold
-  pts = getPoints(mkd, false)
-  nPts = Vector{typeof(u0)}(undef, length(pts))
-  for (i,pt) in enumerate(pts)
-    nPts[i] = setPointPartial!(newM, deepcopy(u0), mkd.manifold, pt, newpartial)
+  # temporary workaround during Manifolds.jl integration
+  manif = convert(Tuple, fullM)[partial_coord_dims]
+  # 
+  newMani = MB.AbstractManifold[]
+  for me in manif
+    push!(newMani, _reducePartialManifoldElements(me))
   end
 
-  # also update metadata elements
-  finalpartial = !isPartial(mkd) ? newpartial : error("not built yet, to shift incoming partial")
-  bw = zeros(manifold_dimension(newM))
-  bw[finalpartial] .= getBW(mkd)[:,1]
-  ipc = zeros(manifold_dimension(newM))
-  ipc[finalpartial] .= getInfoPerCoord(mkd, true)
-
-  manikde!(newM, nPts, u0, bw=bw, partial=finalpartial, infoPerCoord=ipc)
+  # assume independent dimensions for definition, ONLY USED AS DECORATOR AT THIS TIME, FIXME
+  return ProductManifold(newMani...)
 end
 
+"""
+    $SIGNATURES
+
+Return true if this ManifoldKernelDensity is a partial.
+"""
+isPartial(mkd::ManifoldKernelDensity{M,B,L}) where {M,B,L} = true
+isPartial(mkd::ManifoldKernelDensity{M,B,Nothing}) where {M,B} = false
+
 """
     $SIGNATURES
 
@@ -227,6 +186,15 @@ function getPoints( x::ManifoldKernelDensity{M,B,L},
 end
 
 
+function getBW(x::ManifoldKernelDensity{M,B,L}, asPartial::Bool=true; kw...) where {M,B,L}
+  bw = getBW(x.belief; kw...)
+  if L !== Nothing && asPartial
+    return view(bw, x._partial)
+  end
+  return bw
+end
+
+
 # TODO check that partials / marginals are sampled correctly
 function sample(x::ManifoldKernelDensity{M,B,L,P}, N::Int) where {M,B,L,P}
   # get legacy matrix of coordinates and selected labels
@@ -285,9 +253,56 @@ function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P}
   nothing
 end
 
+
 Base.show(io::IO, ::MIME"text/plain", mkd::ManifoldKernelDensity) = show(io, mkd)
 Base.show(io::IO, ::MIME"application/juno.inline", mkd::ManifoldKernelDensity) = show(io, mkd)
 
+
+# override
+function marginal(x::ManifoldKernelDensity{M,B}, 
+                  dims::AbstractVector{<:Integer}  ) where {M <: AbstractManifold , B}
+  #
+  ldims::Vector{Int} = collect(dims)
+  ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0)
+end
+
+function marginal(x::ManifoldKernelDensity{M,B,L}, 
+                  dims::AbstractVector{<:Integer}  ) where {M <: AbstractManifold , B, L <: AbstractVector{<:Integer}}
+  #
+  # @show dims x._partial
+  ldims::Vector{Int} = intersect(x._partial, dims)
+  ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0)
+end
+# manis = convert(Tuple, x.manifold)
+# partMani = _reducePartialManifoldElements(manis[dims])
+# pts = getPoints(x)
+
+
+
+function antimarginal(newM::AbstractManifold,
+                      u0,
+                      mkd::ManifoldKernelDensity, 
+                      newpartial::AbstractVector{<:Integer} )
+  #
+
+  # convert to antimarginal by copying user provided example point for bigger manifold
+  pts = getPoints(mkd, false)
+  nPts = Vector{typeof(u0)}(undef, length(pts))
+  for (i,pt) in enumerate(pts)
+    nPts[i] = setPointPartial!(newM, deepcopy(u0), mkd.manifold, pt, newpartial)
+  end
+
+  # also update metadata elements
+  finalpartial = !isPartial(mkd) ? newpartial : error("not built yet, to shift incoming partial")
+  bw = zeros(manifold_dimension(newM))
+  bw[finalpartial] .= getBW(mkd)[:,1]
+  ipc = zeros(manifold_dimension(newM))
+  ipc[finalpartial] .= getInfoPerCoord(mkd, true)
+
+  manikde!(newM, nPts, u0, bw=bw, partial=finalpartial, infoPerCoord=ipc)
+end
+
+
 ## =======================================================================================
 ##  deprecate as necessary below
 ## =======================================================================================

src/services/ManifoldPartials.jl

@@ -204,9 +204,9 @@ function getManifoldPartial(M::ProductManifold,
 end
 
 function getManifoldPartial(M::GroupManifold, 
-                            partial::AbstractVector{Int}, 
+                            partial::AbstractVector{<:Integer}, 
                             repr::_PartiableRepresentation=nothing,
-                            offset::Base.RefValue{Int}=Ref(0);
+                            offset::Base.RefValue{<:Integer}=Ref(0);
                             doError::Bool=true )
   #
   # mask the desired coordinate dimensions

test/runtests.jl

@@ -11,6 +11,7 @@ include("testManiProductBigSmall.jl")
 include("testBasicManiProduct.jl")
 include("testMarginalProducts.jl")
 include("testMMD.jl")
+include("testPartialProductSE2.jl")
 include("basic_se3.jl")
 
 #

test/testManifoldPartial.jl

@@ -187,8 +187,8 @@ X = manikde!(M, pts, partial=[1;3])
 X_ = replace(X0, X)
 
 # check metadata
-@test  isapprox( getBW(X_)[[1;3],1], getBW(X)[[1;3],1] )
-@test !isapprox( getBW(X_)[[1;3],1], getBW(X0)[[1;3],1] )
+@test  isapprox( getBW(X_, false)[[1;3],1], getBW(X,  false)[[1;3],1] )
+@test !isapprox( getBW(X_, false)[[1;3],1], getBW(X0, false)[[1;3],1] )
 
 @test  isapprox( X_.infoPerCoord[[1;3]], X.infoPerCoord[[1;3]] )