Merge pull request #51 from JuliaRobotics/21Q1/maint/prep020

move to ManifoldBase, bump v0.2.0

Project.toml

@@ -1,7 +1,7 @@
 name = "ApproxManifoldProducts"
 uuid = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 keywords = ["mm-iSAM", "SLAM", "inference", "sum-product", "belief-propagation", "nonparametric", "manifolds", "functional"]
-version = "0.1.5"
+version = "0.2.0"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
@@ -13,6 +13,7 @@ KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
 LibGit2 = "76f85450-5226-5b5a-8eaa-529ad045b433"
 Libdl = "8f399da3-3557-5675-b5ff-fb832c97cbdb"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
 Markdown = "d6f4376e-aef5-505a-96c1-9c027394607a"
 Mmap = "a63ad114-7e13-5084-954f-fe012c677804"
 NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56"
@@ -36,6 +37,7 @@ Compat = "1.5.1, 2, 3.2"
 CoordinateTransformations = "0.5, 0.6, 0.8, 0.9"
 DocStringExtensions = "0.7, 0.8, 0.9"
 KernelDensityEstimate = "0.5.3"
+ManifoldsBase = "0.10"
 NLsolve = "3, 4"
 Optim = "0.16, 0.17, 0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 1"
 Reexport = "0.2, 1.0"

src/API.jl

@@ -31,14 +31,15 @@ plot( x=getPoints(pq)[1,:], y=getPoints(pq)[2,:], Geom.histogram2d )
 """
 function manifoldProduct( ff::Vector{BallTreeDensity},
                           manif::T;
-                          Niter=1,
+                          makeCopy::Bool=false,
+                          Niter::Int=1,
                           addEntropy::Bool=true,
                           recordLabels::Bool=false,
                           selectedLabels::Vector{Vector{Int}}=Vector{Vector{Int}}()) where {T <: Tuple}
   #
   # check quick exit
   if 1 == length(ff)
-    return ff[1]
+    return (makeCopy ? x->deepcopy(x) : x->x)(ff[1])
   end
 
   ndims = Ndim(ff[1])
@@ -55,7 +56,7 @@ function manifoldProduct( ff::Vector{BallTreeDensity},
   glbs.recordChoosen = recordLabels
 
   pGM, = prodAppxMSGibbsS(dummy, ff,
-                          nothing, nothing, Niter=1,
+                          nothing, nothing, Niter=Niter,
                           addop=addopT,
                           diffop=diffopT,
                           getMu=getManiMu,

src/ApproxManifoldProducts.jl

@@ -3,6 +3,9 @@ module ApproxManifoldProducts
 using Reexport
 @reexport using KernelDensityEstimate
 @reexport using TransformUtils
+import ManifoldsBase
+
+const MB = ManifoldsBase
 
 using DocStringExtensions
 
@@ -17,6 +20,7 @@ import Base: *, isapprox
 # import KernelDensityEstimate: kde!
 import LinearAlgebra: rotate!
 
+
 const AMP = ApproxManifoldProducts
 const KDE = KernelDensityEstimate
 const TUs = TransformUtils
@@ -25,7 +29,8 @@ const CTs = CoordinateTransformations
 # TODO temporary for initial version of on-manifold products
 KDE.setForceEvalDirect!(true)
 
-export
+export  
+  # new local features
   AMP,
   get2DLambda,
   get2DMu,
@@ -46,7 +51,6 @@ export
   manikde!,
 
   # general manifolds
-  Manifold,
   Circular
 
 # internal features not exported

src/Circular.jl

@@ -4,21 +4,21 @@
 
 
 
-struct Circular <: Manifold
+struct Circular <: MB.Manifold{MB.ℝ}
   dof::Int
   addop::Function
   diffop::Function
   getMu
   getLambda
   domain::Tuple{Float64, Float64}
-  Circular() = new(1,
-                   addtheta,
-                   difftheta,
-                   getCircMu,
-                   getCircLambda,
-                   (-pi+0.0,pi-1e-15))
 end
 
+Circular() = Circular(1,
+                      addtheta,
+                      difftheta,
+                      getCircMu,
+                      getCircLambda,
+                      (-pi+0.0,pi-1e-15))
 
 
 

src/Euclidean.jl

@@ -1,31 +1,28 @@
 # Euclidean Manifold definitions and arithmetic
 
 
-struct Euclid2 <: Manifold
+struct Euclid2 <: MB.Manifold{MB.ℝ}
   dof::Int
   addop::Function
   diffop::Function
   getMu
   getLambda
   domain::Tuple{Tuple{Float64,Float64},Tuple{Float64, Float64}}
-  Euclid2() = new(2, +, -, KDE.getEuclidMu, KDE.getEuclidLambda, ((-Inf,Inf),(-Inf,Inf)))
 end
 
+Euclid2() = Euclid2(2, +, -, KDE.getEuclidMu, KDE.getEuclidLambda, ((-Inf,Inf),(-Inf,Inf)))
+
 # ??
-struct EuclideanManifold <: Manifold
+struct EuclideanManifold <: MB.Manifold{MB.ℝ}
 end
 
 
 
-get2DLambda(Lambdas::Vector{Float64})::Float64 = sum(Lambdas)
-
+get2DLambda(Lambdas::AbstractVector{<:Real}) = sum(Lambdas)
 
 
-function *(PP::Vector{MB{EuclideanManifold,BallTreeDensity}})
-  bds = Vector{BallTreeDensity}(undef, length(PP))
-  for p in PP
-    bds[i] = p.belief
-  end
+function *(PP::Vector{MB_{EuclideanManifold,BallTreeDensity}})
+  bds = (p->p.belief).(PP)
   *(bds)
 end
 

src/Interface.jl

@@ -1,18 +1,14 @@
 # Interface
 
-
-abstract type Manifold end
-
-
-mutable struct ManifoldBelief{M <: Manifold, B}
+mutable struct ManifoldBelief{M <: MB.Manifold{MB.ℝ}, B}
   manifold::Type{M}
   belief::B
 end
-ManifoldBelief(m::M,b::B) where {M <: Manifold, B} = ManifoldBelief{M,B}
+ManifoldBelief(m::M,b::B) where {M <: MB.Manifold{MB.ℝ}, B} = ManifoldBelief{M,B}
 
-const MB{M,B} = ManifoldBelief{M, B}
+const MB_{M,B} = ManifoldBelief{M, B}
 
-function *(PP::Vector{MB{M,B}}) where {M<:Manifold,B}
+function *(PP::Vector{MB_{M,B}}) where {M<:MB.Manifold{MB.ℝ},B}
   @info "taking manifold product of $(length(PP)) terms, $M, $B"
   @error "No known product definition"
 end

src/KernelHilbertEmbeddings.jl

@@ -16,17 +16,17 @@ export
 # abstract type ManifoldDefs end
 #
 # FIXME standardize with Manifolds.jl
-struct Euclid <: Manifold end
-struct SE2_Manifold <: Manifold end
-struct SE3_Manifold <: Manifold end
-
-function ker(::Type{Euclid},
-             x::AbstractArray{<:Real,2},
-             y::AbstractArray{<:Real,2},
-             dx::Vector{<:Real},
-             i::Int,
-             j::Int;
-             sigma::Real=0.001 )
+struct Euclid <: MB.Manifold{MB.ℝ} end
+struct SE2_Manifold <: MB.Manifold{MB.ℝ} end
+struct SE3_Manifold <: MB.Manifold{MB.ℝ} end
+
+function ker( ::Type{Euclid},
+              x::AbstractArray{<:Real,2},
+              y::AbstractArray{<:Real,2},
+              dx::Vector{<:Real},
+              i::Int,
+              j::Int;
+              sigma::Real=0.001 )
   #
   dx[1] = x[1,i]
   dx[1] -= y[1,j]
@@ -35,13 +35,13 @@ function ker(::Type{Euclid},
   exp( dx[1] )
 end
 
-function ker(::Type{Euclid2},
-             x::AbstractArray{<:Real,2},
-             y::AbstractArray{<:Real,2},
-             dx::Vector{<:Real},
-             i::Int,
-             j::Int;
-             sigma::Real=0.001 )
+function ker( ::Type{Euclid2},
+              x::AbstractArray{<:Real,2},
+              y::AbstractArray{<:Real,2},
+              dx::Vector{<:Real},
+              i::Int,
+              j::Int;
+              sigma::Real=0.001 )
   #
   dx[1] = x[1,i]
   dx[2] = x[2,i]
@@ -53,27 +53,27 @@ function ker(::Type{Euclid2},
   exp( dx[1] )
 end
 
-function ker(::Type{SE2_Manifold},
-             x::AbstractMatrix{<:Real},
-             y::AbstractMatrix{<:Real},
-             dx::Vector{<:Real},
-             i::Int,
-             j::Int;
-             sigma::Real=0.001  )
+function ker( ::Type{SE2_Manifold},
+              x::AbstractMatrix{<:Real},
+              y::AbstractMatrix{<:Real},
+              dx::Vector{<:Real},
+              i::Int,
+              j::Int;
+              sigma::Real=0.001  )
   #
   innov = se2vee(SE2(x[:,i])\SE2(y[:,j]))
   exp( -sigma*(  innov'*innov  ) )
 end
 
 # This functin is still very slow, needs speedup
 # Obviously want to get away from the Euler angles throughout
-function ker(::Type{SE3_Manifold},
-             x::AbstractMatrix{<:Real},
-             y::AbstractMatrix{<:Real},
-             dx::Vector{<:Real},
-             i::Int,
-             j::Int;
-             sigma::Real=0.001  )
+function ker( ::Type{SE3_Manifold},
+              x::AbstractMatrix{<:Real},
+              y::AbstractMatrix{<:Real},
+              dx::Vector{<:Real},
+              i::Int,
+              j::Int;
+              sigma::Real=0.001  )
   #
   innov = veeEuler(SE3(x[1:3,i],Euler((x[4:6,i])...))\SE3(y[1:3,j],Euler((y[4:6,j])...)))
   exp( -sigma*(  innov'*innov  ) )
@@ -89,7 +89,7 @@ Notes:
 - This is the in-place version (well more in-place than mmd)
 
 DevNotes:
-- TODO make work for different sizes
+- TODO make work for different sizes N,M
 - TODO dont assume equally weighted particles
 
 Related
@@ -99,18 +99,19 @@ mmd, ker
 function mmd!(val::AbstractVector{<:Real},
               a::AbstractArray{<:Real,2},
               b::AbstractArray{<:Real,2},
-              mani::Type{<:Manifold}=Euclid,
-              N::Int=size(a,2), M::Int=size(b,2); bw::Vector{<:Real}=[0.001;] )
+              mani::Type{<: MB.Manifold}=Euclid,
+              N::Int=size(a,2), M::Int=size(b,2); 
+              bw::AbstractVector{<:Real}=[0.001;] )
   #
   # TODO allow unequal data too
   @assert N == M
   # reci_len = 1.0/N
   val[1] = 0.0
   dx = zeros(2)
   @inbounds @fastmath for i in 1:N
-     @simd for j in 1:M
-       val[1] -= ker(mani, a, b, dx, i, j, sigma=bw[1])
-     end
+    @simd for j in 1:M
+      val[1] -= ker(mani, a, b, dx, i, j, sigma=bw[1])
+    end
   end
   val .*= 2.0
   @inbounds @fastmath for i in 1:N
@@ -136,15 +137,16 @@ Related
 
 mmd!, ker
 """
-function mmd(a::AbstractArray{<:Real,2},
-             b::AbstractArray{<:Real,2},
-             mani::Type{<:Manifold}=Euclid,
-             N::Int=size(a,2), M::Int=size(b,2); bw::Vector{<:Real}=[0.001;])
+function mmd( a::AbstractArray{<:Real,2},
+              b::AbstractArray{<:Real,2},
+              mani::Type{<: MB.Manifold}=Euclid,
+              N::Int=size(a,2), M::Int=size(b,2); 
+              bw::AbstractVector{<:Real}=[0.001;])
   #
   val = [0.0;]
-  mmd!(val, a,b,
-       mani,
-       N, M; bw=bw )
+  mmd!( val, a,b,
+        mani,
+        N, M; bw=bw )
   #
   return val[1]
 end