Merge pull request #250 from JuliaRobotics/23Q4/docs/andcleanup

some docs and cleanup
JuliaRobotics · Oct 9, 2023 · fa3454d · fa3454d
2 parents 8bf8254 + 6c7246e
commit fa3454d
Show file tree

Hide file tree

Showing 2 changed files with 71 additions and 31 deletions.
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...
+)
+
 #
 
 
@@ -188,7 +215,8 @@ Random.rand(mkd::ManifoldKernelDensity, N::Integer=1) = sample(mkd, N)[1]
 
 function resample(x::ManifoldKernelDensity, N::Int)
   pts = if N < Npts(x)
-    shuffle(getPoints(x))[1:N]
+    # 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