Merge pull request #65 from JuliaRobotics/21Q1/feat/pointsmani

getPointsManifold and Manifolds objects (not types)

Project.toml

@@ -1,11 +1,10 @@
 name = "ApproxManifoldProducts"
 uuid = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 keywords = ["MM-iSAMv2", "SLAM", "inference", "sum-product", "belief-propagation", "nonparametric", "manifolds", "functional"]
-version = "0.3.0"
+version = "0.3.1"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
-Compat = "34da2185-b29b-5c13-b0c7-acf172513d20"
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
 Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
@@ -29,13 +28,12 @@ Sockets = "6462fe0b-24de-5631-8697-dd941f90decc"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
-Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 Unicode = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5"
 
 [compat]
-Compat = "1.5.1, 2, 3.2"
 CoordinateTransformations = "0.5, 0.6, 0.8, 0.9"
 DocStringExtensions = "0.7, 0.8, 0.9"
 JSON2 = "0.3, 0.4, 0.5, 0.6, 0.7, 1"
@@ -48,6 +46,7 @@ Reexport = "0.2, 1.0"
 Requires = "0.5, 0.6, 0.7, 0.8, 0.9, 0.10, 1"
 SLEEFPirates = "0.5, 0.6"
 StaticArrays = "0.11, 0.12, 0.13, 0.14, 0.15, 1"
+TensorCast = "0.2, 0.3, 0.4"
 TransformUtils = "^0.2.2"
 julia = "1.4"
 

src/ApproxManifoldProducts.jl

@@ -19,6 +19,7 @@ using Requires
 using SLEEFPirates
 using LinearAlgebra
 using JSON2
+using TensorCast
 
 import Base: *, isapprox, convert
 # import KernelDensityEstimate: kde!

src/Euclidean.jl

@@ -18,7 +18,7 @@
 get2DLambda(Lambdas::AbstractVector{<:Real}) = sum(Lambdas)
 
 
-function *(PP::AbstractVector{<:MKD{EuclideanManifold,BallTreeDensity}})
+function *(PP::AbstractVector{<:MKD{typeof(EuclideanManifold),BallTreeDensity}})
   bds = (p->p.belief).(PP)
   *(bds)
 end

src/Interface.jl

@@ -1,29 +1,29 @@
 # Interface
 
 mutable struct ManifoldKernelDensity{M <: MB.Manifold{MB.ℝ}, B <: BallTreeDensity}
-  manifold::Type{M}
+  manifold::M
   belief::B
 end
 const MKD{M,B} = ManifoldKernelDensity{M, B}
 
 # ManifoldKernelDensity(m::M,b::B) where {M <: MB.Manifold{MB.ℝ}, B} = ManifoldKernelDensity{M,B}(m,b)
 
 
-function ManifoldKernelDensity(m::Type{<:MB.Manifold}, pts::AbstractArray{<:Real})
+function ManifoldKernelDensity(m::MB.Manifold, pts::AbstractArray{<:Real})
   tup = convert(Tuple, m)
   bel = manikde!(pts, m)
   ManifoldKernelDensity(m, bel)
 end
 
 
 @deprecate ManifoldBelief(w...;kw...) ManifoldKernelDensity(w...;kw...)
-function ManifoldBelief(::Type{<:M}, mkd::ManifoldKernelDensity{M,T}) where {M <: MB.Manifold{MB.ℝ}, T} 
+function ManifoldBelief(::M, mkd::ManifoldKernelDensity{M,T}) where {M <: MB.Manifold{MB.ℝ}, T} 
   @warn "ManifoldBelief is deprecated, use ManifoldKernelDensity instead"
   return mkd
 end
 
 function Base.show(io::IO, mkd::ManifoldKernelDensity{M,B}) where {M, B}
-  printstyled(io, "ManifoldKernelDensity{$M,$B}(\n", bold=true)
+  printstyled(io, "ManifoldKernelDensity{$M,$B}(\n", bold=true )
   show(io, mkd.belief)
   println(io, ")")
 end
@@ -82,6 +82,7 @@ function Base.convert(::Type{<:ManifoldKernelDensity}, str::AbstractString)
 end
 
 
+
 ## ================================================================================================================================
 # pass through API
 ## ================================================================================================================================

src/KernelHilbertEmbeddings.jl

@@ -6,7 +6,7 @@ export
   mmd
 
 
-function ker( ::Type{Euclid},
+function ker( ::typeof(Euclidean(1)),
               x::AbstractArray{<:Real,2},
               y::AbstractArray{<:Real,2},
               dx::Vector{<:Real},
@@ -21,7 +21,7 @@ function ker( ::Type{Euclid},
   exp( dx[1] )
 end
 
-function ker( ::Type{Euclid2},
+function ker( ::typeof(Euclidean(2)),
               x::AbstractArray{<:Real,2},
               y::AbstractArray{<:Real,2},
               dx::Vector{<:Real},
@@ -39,7 +39,7 @@ function ker( ::Type{Euclid2},
   exp( dx[1] )
 end
 
-function ker( ::Type{SE2_Manifold},
+function ker( ::typeof(SE2_Manifold),
               x::AbstractMatrix{<:Real},
               y::AbstractMatrix{<:Real},
               dx::Vector{<:Real},
@@ -53,7 +53,7 @@ end
 
 # This functin is still very slow, needs speedup
 # Obviously want to get away from the Euler angles throughout
-function ker( ::Type{SE3_Manifold},
+function ker( ::typeof(SE3_Manifold),
               x::AbstractMatrix{<:Real},
               y::AbstractMatrix{<:Real},
               dx::Vector{<:Real},
@@ -85,7 +85,7 @@ mmd, ker
 function mmd!(val::AbstractVector{<:Real},
               a::AbstractArray{<:Real,2},
               b::AbstractArray{<:Real,2},
-              mani::Type{<: MB.Manifold}=Euclid,
+              mani::MB.Manifold=Euclid,
               N::Int=size(a,2), M::Int=size(b,2); 
               bw::AbstractVector{<:Real}=[0.001;] )
   #
@@ -125,7 +125,7 @@ mmd!, ker
 """
 function mmd( a::AbstractArray{<:Real,2},
               b::AbstractArray{<:Real,2},
-              mani::Type{<: MB.Manifold}=Euclid,
+              mani::MB.Manifold=Euclid,
               N::Int=size(a,2), M::Int=size(b,2); 
               bw::AbstractVector{<:Real}=[0.001;])
   #

src/Legacy.jl

@@ -39,14 +39,19 @@ function buildHybridManifoldCallbacks(manif::Tuple)
 end
 
 # FIXME temp conversion during consolidation
-Base.convert(::Type{<:Tuple}, mani::Type{<: Euclid}) = (:Euclid,)
-Base.convert(::Type{<:Tuple}, mani::Type{<: Euclid2}) = (:Euclid,:Euclid)
-Base.convert(::Type{<:Tuple}, mani::Type{<: Euclid3}) = (:Euclid,:Euclid,:Euclid)
-Base.convert(::Type{<:Tuple}, mani::Type{<: Euclid4}) = (:Euclid,:Euclid,:Euclid,:Euclid)
-Base.convert(::Type{<:Tuple}, mani::Type{<: SE2_Manifold}) = (:Euclid,:Euclid,:Circular)
-Base.convert(::Type{<:Tuple}, mani::Type{<: SE2E2_Manifold}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
-Base.convert(::Type{<:Tuple}, mani::Type{<: SE3_Manifold}) = (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular)
-
+Base.convert(::Type{<:Tuple}, ::Type{<: typeof(Euclid)}) = (:Euclid,)
+Base.convert(::Type{<:Tuple}, ::Type{<: typeof(Euclid2)}) = (:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, ::Type{<: typeof(Euclid3)}) = (:Euclid,:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, ::Type{<: typeof(Euclid4)}) = (:Euclid,:Euclid,:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, ::Type{<: typeof(SE2_Manifold)}) = (:Euclid,:Euclid,:Circular)
+Base.convert(::Type{<:Tuple}, ::Type{<: typeof(SE3_Manifold)}) = (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular)
+
+Base.convert(::Type{<:Tuple}, ::typeof(Euclid)) = (:Euclid,)
+Base.convert(::Type{<:Tuple}, ::typeof(Euclid2)) = (:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, ::typeof(Euclid3)) = (:Euclid,:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, ::typeof(Euclid4)) = (:Euclid,:Euclid,:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, ::typeof(SE2_Manifold)) = (:Euclid,:Euclid,:Circular)
+Base.convert(::Type{<:Tuple}, ::typeof(SE3_Manifold)) = (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular)
 
 
 """

src/ManifoldDefinitions.jl

@@ -4,6 +4,9 @@
 
 export 
   # general manifolds
+  Euclidean,
+  Circle,
+  ℝ,
   Euclid,
   Euclid2,
   Euclid3,
@@ -12,30 +15,32 @@ export
   SE3_Manifold,
 
   # special quirks that still need to be fixed
-  Circle1,
+  Circle1
   # Circular,
-  SE2E2_Manifold
 
-#
+export
+  coords,
+  uncoords,
+  getPointsManifold,
+  calcMean
 
+#
 
-# this is a hack
-struct SE2E2_Manifold <: MB.Manifold{MB.ℝ} end
-# const SE2E2_Manifold = 
 
 # this is just wrong and needs to be fixed
-const Circle1 = Circle{ℝ}
+const Circle1 = Circle()
 
-const Euclid =  Euclidean{Tuple{1}, ℝ} # Euclidean(1)
+const Euclid =  Euclidean(1) # Euclidean{Tuple{1}, ℝ} #
 const EuclideanManifold = Euclid
 
-const Euclid2 = Euclidean{Tuple{2}, ℝ} # Euclidean(2)
-const Euclid3 = Euclidean{Tuple{3}, ℝ} # Euclidean(3)
-const Euclid4 = Euclidean{Tuple{4}, ℝ} # Euclidean(4)
+const Euclid2 = Euclidean(2) # Euclidean{Tuple{2}, ℝ} #
+const Euclid3 = Euclidean(3) # Euclidean{Tuple{3}, ℝ} #
+const Euclid4 = Euclidean(4) # Euclidean{Tuple{4}, ℝ}
+
+# TODO if not easy simplification exists, then just deprecate this
+const SE2_Manifold = SpecialEuclidean(2)
+const SE3_Manifold = SpecialEuclidean(3)
 
-const SE2_Manifold = typeof(SpecialEuclidean(2))
-const SE3_Manifold = typeof(SpecialEuclidean(3))
-
 
 Base.convert(::Type{B}, mkd::ManifoldKernelDensity{M,B}) where {M,B<:BallTreeDensity} = mkd.belief
 
@@ -58,3 +63,74 @@ Base.convert(::Type{B}, mkd::ManifoldKernelDensity{M,B}) where {M,B<:BallTreeDen
 # const EuclideanManifold = Euclid
 # # @deprecate EuclideanManifold() Euclid() 
 # # struct EuclideanManifold <: MB.Manifold{MB.ℝ} end
+
+
+
+##============================================================================================================
+## New Manifolds.jl aware API -- TODO find the right file placement
+##============================================================================================================
+
+
+function getPointsManifold(mkd::ManifoldKernelDensity{M}) where {M <: Euclidean}
+  data_ = getPoints(mkd.belief)
+  TensorCast.@cast data[i][j] := data_[j,i]
+  return data
+end
+
+function getPointsManifold(mkd::ManifoldKernelDensity{M}) where {M <: Circle}
+  data_ = getPoints(mkd.belief)
+  return data_[:]
+end
+
+coords(::Type{<:typeof(SpecialEuclidean(2))}, p::ProductRepr) = [p.parts[1][1], p.parts[1][2], atan(p.parts[2][2,1],p.parts[2][1,1])]
+
+function uncoords(::Type{<:typeof(SpecialEuclidean(2))}, p::AbstractVector{<:Real}, static::Bool=true)
+  α = p[3] 
+  ArrConst = static ? SA : eltype(α)
+  return ProductRepr((ArrConst[p[1], p[2]]), ArrConst[cos(α) -sin(α); sin(α) cos(α)])
+end
+# function uncoords(::Type{<:typeof(SpecialEuclidean(2))}, p::AbstractVector{<:Real})
+#   α = p[3]
+#   return ProductRepr(([p[1], p[2]]), [cos(α) -sin(α); sin(α) cos(α)])
+# end
+
+function coords(::Type{<:typeof(SpecialEuclidean(3))}, p::ProductRepr)
+  wELo = TU.convert(Euler, SO3(p.parts[2]))
+  [p.parts[1][1:3]; wELo.R; wELo.P; wELo.Y]
+end
+
+function uncoords(::Type{<:typeof(SpecialEuclidean(3))}, p::AbstractVector{<:Real})
+  # α = p[3]
+  wRo = TU.convert(SO3, Euler(p[4:6]...))
+  return ProductRepr(([p[1], p[2], p[3]]), wRo.R)
+end
+
+function getPointsManifold(mkd::ManifoldKernelDensity{M}) where {M <: SpecialEuclidean}
+  data_ = getPoints(mkd.belief)
+  [uncoords(M, view(data_, :, i)) for i in 1:size(data_,2)]
+end
+
+
+# TODO, hack, use the proper Manifolds.jl intended vectoration methods instead
+_makeVectorManifold(::Manifold, arr::AbstractArray{<:Real}) = arr
+_makeVectorManifold(::Manifold, val::Real) = [val;]
+_makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(SpecialEuclidean(2))} = coords(M, prr)
+_makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(SpecialEuclidean(3))} = coords(M, prr)
+
+
+function calcMean(mkd::ManifoldKernelDensity{M}) where {M <: ManifoldsBase.Manifold}
+  data = getPointsManifold(mkd)
+  mprepr = mean(mkd.manifold, data)
+
+  #
+  _makeVectorManifold(mkd.manifold, mprepr)
+end
+
+
+
+
+
+
+
+
+#