Merge pull request #251 from JuliaRobotics/master

v0.8.1-rc1
JuliaRobotics · Oct 9, 2023 · 9d83875 · 9d83875
2 parents bd5c45c + 1c3eb2e
commit 9d83875
Show file tree

Hide file tree

Showing 4 changed files with 88 additions and 42 deletions.
diff --git 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.0"
+version = "0.8.1"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

diff --git a/src/API.jl b/src/API.jl
@@ -112,18 +112,19 @@ function manifoldProduct( ff::AbstractVector{<:ManifoldKernelDensity},
   ## TODO check both _ff and inplace use a matrix of coordinates (columns)
   # expects Matrix with columns as samples and rows are coordinate dimensions
   pGM, = prodAppxMSGibbsS(inplace, _ff,
-                          nothing, nothing, Niter=Niter,
-                          partialDimMask=partialDimMask,
+                          nothing, nothing; 
+                          Niter,
+                          partialDimMask,
                           addop=addopT,
                           diffop=diffopT,
                           getMu=getManiMu,
-                          glbs=glbs,
-                          addEntropy=addEntropy,
-                          ndims=ndims,
-                          Ndens=Ndens,
-                          Np=Np,
-                          maxNp=maxNp,
-                          Nlevels=Nlevels,
+                          glbs,
+                          addEntropy,
+                          ndims,
+                          Ndens,
+                          Np,
+                          maxNp,
+                          Nlevels,
                           randU=_randU,
                           randN=_randN  );
   #

diff --git a/src/entities/ManifoldKernelDensity.jl b/src/entities/ManifoldKernelDensity.jl
@@ -1,5 +1,17 @@
 
 
+"""
+    $TYPEDEF
+
+On-manifold kernel density belief.
+
+Notes
+- Allows partials as identified by list of coordinate dimensions e.g. `._partial = [1;3]`
+  - When building a partial belief, use full points with necessary information in the specified partial coords.
+
+DevNotes
+- WIP AMP issue 41, use generic retractions during manifold products.
+"""
 struct ManifoldKernelDensity{M <: MB.AbstractManifold, B <: BallTreeDensity, L, P <: AbstractArray}
   manifold::M
   """ legacy expects matrix of coordinates (as columns) """

diff --git a/src/services/ManifoldKernelDensity.jl b/src/services/ManifoldKernelDensity.jl
@@ -4,18 +4,28 @@
 ## ==========================================================================================
 
 
-ManifoldKernelDensity(mani::M, 
-                      bel::B, 
-                      ::Nothing=nothing, 
-                      u0::P=zeros(manifold_dimension(mani));
-                      infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(mani, u0)) ) where {M <: MB.AbstractManifold, B <: BallTreeDensity, P} = ManifoldKernelDensity{M,B,Nothing,P}(mani,bel,nothing,u0,infoPerCoord)
+ManifoldKernelDensity(
+  mani::M, 
+  bel::B, 
+  ::Nothing=nothing, 
+  u0::P=zeros(manifold_dimension(mani));
+  infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(mani, u0)) 
+) where {M <: MB.AbstractManifold, B <: BallTreeDensity, P} = ManifoldKernelDensity{M,B,Nothing,P}(
+  mani,
+  bel,
+  nothing,
+  u0,
+  infoPerCoord
+)
 #
 
-function ManifoldKernelDensity( mani::M, 
-                                bel::B, 
-                                partial::L, 
-                                u0::P=zeros(manifold_dimension(mani));
-                                infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(mani, u0)) ) where {M <: MB.AbstractManifold, B <: BallTreeDensity, L<:AbstractVector{<:Integer}, P}
+function ManifoldKernelDensity( 
+  mani::M, 
+  bel::B, 
+  partial::L, 
+  u0::P=zeros(manifold_dimension(mani));
+  infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(mani, u0)) 
+) where {M <: MB.AbstractManifold, B <: BallTreeDensity, L<:AbstractVector{<:Integer}, P}
   #
   if length(partial) != manifold_dimension(mani)
     # call the constructor direct
@@ -26,26 +36,35 @@ function ManifoldKernelDensity( mani::M,
   end
 end
 
-ManifoldKernelDensity(mani::M, 
-                      bel::B, 
-                      pl_mask::Union{<:BitVector,<:AbstractVector{<:Bool}}, 
-                      u0::P=zeros(manifold_dimension(mani));
-                      infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(mani, u0)) ) where {M <: MB.AbstractManifold, B <: BallTreeDensity, P} = ManifoldKernelDensity(mani,bel,(1:manifold_dimension(mani))[pl_mask],u0;infoPerCoord=infoPerCoord)
-
-
-function ManifoldKernelDensity( M::MB.AbstractManifold,
-                                vecP::AbstractVector{P},
-                                u0=vecP[1],
-                                ϵ = identity_element(M, vecP[1]);
-                                partial::L=nothing,
-                                infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(M, u0)),
-                                dims::Int=manifold_dimension(M),
-                                bw::Union{<:AbstractVector{<:Real},Nothing}=nothing  ) where {P,L}
+ManifoldKernelDensity(
+  mani::M, 
+  bel::B, 
+  pl_mask::Union{<:BitVector,<:AbstractVector{<:Bool}}, 
+  u0::P=zeros(manifold_dimension(mani));
+  infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(mani, u0))
+) where {M <: MB.AbstractManifold, B <: BallTreeDensity, P} = ManifoldKernelDensity(
+  mani,
+  bel,
+  (1:manifold_dimension(mani))[pl_mask],
+  u0;
+  infoPerCoord
+)
+
+
+function ManifoldKernelDensity( 
+  M::MB.AbstractManifold,
+  vecP::AbstractVector{P},
+  u0 = vecP[1],
+  ϵ = identity_element(M, u0); # vecP[1]
+  partial::L=nothing,
+  infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(M, u0)),
+  dims::Int=manifold_dimension(M),
+  bw::Union{<:AbstractVector{<:Real},Nothing}=nothing  
+) where {P,L}
   #
   # FIXME obsolete
   arr = Matrix{Float64}(undef, dims, length(vecP))
 
-
   for j in 1:length(vecP)
     arr[:,j] = vee(M, ϵ, log(M, ϵ, vecP[j]))
   end
@@ -66,10 +85,18 @@ end
 
 
 # MAYBE deprecate name
-manikde!( M::MB.AbstractManifold,
-          vecP::AbstractVector{P},
-          u0::P=vecP[1];
-          kw... ) where P = ManifoldKernelDensity(M, vecP, u0; kw...) 
+manikde!( 
+  M::MB.AbstractManifold,
+  vecP::AbstractVector{P},
+  u0::P=vecP[1];
+  kw... 
+) where P = ManifoldKernelDensity(
+  M, 
+  vecP, 
+  u0; 
+  kw...
+)
+
 #
 
 
@@ -187,7 +214,13 @@ Random.rand(mkd::ManifoldKernelDensity, N::Integer=1) = sample(mkd, N)[1]
 
 
 function resample(x::ManifoldKernelDensity, N::Int)
-  pts, = sample(x, N)
+  pts = if N < Npts(x)
+    # get points with non-partial coord dims so that new MKD can be built
+    shuffle(getPoints(x, false))[1:N]
+  else
+    _pts, = sample(x, N)
+    _pts
+  end
   ManifoldKernelDensity(x.manifold, pts, x._u0, partial=x._partial, infoPerCoord=x.infoPerCoord)
 end
 
@@ -296,7 +329,7 @@ function antimarginal(newM::AbstractManifold,
   ipc = zeros(manifold_dimension(newM))
   ipc[finalpartial] .= getInfoPerCoord(mkd, true)
 
-  manikde!(newM, nPts, u0, bw=bw, partial=finalpartial, infoPerCoord=ipc)
+  manikde!(newM, nPts, u0; bw, partial=finalpartial, infoPerCoord=ipc)
 end