Merge pull request #130 from JuliaRobotics/master

v0.4.11-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.10"
+version = "0.4.11"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

src/services/ManifoldKernelDensity.jl

@@ -45,15 +45,15 @@ function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B,L,P}) where {M,B,L,P}
   print(io, "  dims:  ", Ndim(mkd.belief))
   printstyled(io, isPartial(mkd) ? "* --> $(length(mkd._partial))" : "", bold=true)
   println(io)
-  println(io, "  prtl:  ", mkd._partial)
+  println(io, "  prtl:   ", mkd._partial)
   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)
-    println(io, "   mean:  ", round.(mn',digits=4))
+    println(io, "   mean: ", round.(mn',digits=4))
   catch
   end
   println(io, ")")
@@ -144,10 +144,11 @@ function marginal(x::ManifoldKernelDensity{M,B},
 end
 
 function marginal(x::ManifoldKernelDensity{M,B,L}, 
-  dims::AbstractVector{<:Integer}  ) where {M <: AbstractManifold , B, L <: AbstractVector{<:Integer}}
-#
-ldims::Vector{Int} = collect(L[dims])
-ManifoldKernelDensity(x.manifold, x.belief, ldims, x._u0)
+                  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])

src/services/ManifoldPartials.jl

@@ -112,8 +112,56 @@ function getManifoldPartial(M::typeof(SpecialOrthogonal(2)),
   return (M,repr)
 end
 
-# assumed to follow coordinates as transition step towards more general solution
-# assume ProductManifold is the only way to stitch multiple manifolds together
+"""
+    $SIGNATURES
+
+A so-called full dimension manifold can possibly be reduced to smaller partial manifolds over 
+some of the dimensions, returning a new programatically generated `<:AbstractManifold`.
+This funciton can optionally also reduce a point representation for the desired 
+partial dimensions too.
+
+Example
+```julia
+using Manifolds
+using ApproxManifoldProducts
+
+# a familiar manifold of translation and rotation in 2D
+M = SpecialEuclidean(2)
+
+
+# make a new partial of only the translation components
+M_x, _ = getManifoldPartial(M,[1;])
+# returns a new TranslationGroup(1) corresponding to just x dimension
+
+# representation is semidirect product of translation and rotation matrix
+u0 = ProductRepr([0.0;0],[1 0; 0 1.0])
+
+# known coordinates are [x,y,θ], eg
+#   vee(M,identity(M,u0),log(M,identity(M,u0),u0))
+#   [0;0;0] in this example
+
+# make another new partial of only the rotation component
+M_rot, u_rot = getManifoldPartial(M,[3],u0)
+# returns SpecialOrthogonal(2) information
+
+# make another new partial of only  y and θ
+M_yθ, u_yθ = getManifoldPartial(M,[2;3],u0)
+# returns new manifold information as ProductArray(TranslationGroup(1),SpecialOrthogonal(2))
+```
+
+Notes
+- Partial dimensions of interest are defined via a `AbstractVector{Int}` of coordinate dimensions.
+- assumed to follow coordinates as transition step towards more general solution
+- assume ProductManifold is the only way to stitch multiple manifolds together
+- Still experimental.
+
+DevNotes
+- FIXME any semidirect product or action information is lost and naive Product manifold assumptions are currently made.
+
+Related
+
+[`getManifold`](@ref), [`AbstractManifold`], [`ProductManifold`], [`GroupManifold`], [`ProductRepr`]
+"""
 function getManifoldPartial(M::ProductManifold, 
                             partial::AbstractVector{Int}, 
                             repr::_PartiableRepresentationProduct=nothing,

test/testManifoldPartial.jl

@@ -137,4 +137,32 @@ p12 = getPoints(P12)
 end
 
 
+@testset "test marginal of marginal (partial) helper" begin
+##
+
+M = TranslationGroup(3)
+pts = [randn(3) for _ in 1:75]
+
+X = manikde!(M, pts, partial=[1;3])
+
+X_ = marginal(X, [3])
+
+ps3 = getPoints(X_)
+
+for (i,pt) in enumerate(pts)
+  @test isapprox(ps3[i][1], pt[3])
+end
+
+try
+  M = TranslationGroup(4)
+  # check the constructor when only a few points are available
+  X = manikde(M, pts, partial=[1;3;4])
+catch
+  @test_broken false
+end
+
+##
+end
+
+
 #