Merge pull request #1209 from JuliaRobotics/21Q1/refac/tomanijl

add dep Manifolds.jl, towards new defs

Project.toml

@@ -23,6 +23,7 @@ JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Logging = "56ddb016-857b-54e1-b83d-db4d58db5568"
+Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
 MetaGraphs = "626554b9-1ddb-594c-aa3c-2596fe9399a5"
 NLSolversBase = "d41bc354-129a-5804-8e4c-c37616107c6c"
@@ -40,7 +41,7 @@ TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 
 [compat]
-ApproxManifoldProducts = "0.3"
+ApproxManifoldProducts = "0.3.1"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
@@ -52,6 +53,7 @@ FunctionalStateMachine = "0.2.9"
 JLD2 = "0.2, 0.3, 0.4"
 JSON2 = "0.3, 0.4, 0.5, 0.6, 0.7, 1"
 KernelDensityEstimate = "0.5.1, 0.6"
+Manifolds = "0.4"
 ManifoldsBase = "0.10"
 MetaGraphs = "0.6.4"
 NLSolversBase = "7.6"

src/Deprecated.jl

@@ -21,72 +21,38 @@ function _evalType(pt::String)::Type
 end
 
 
-"""
-$(TYPEDEF)
-
-TODO TO BE DEPRECATED
-"""
-mutable struct PotProd
-    Xi::Symbol # Int
-    prev::Array{Float64,2}
-    product::Array{Float64,2}
-    potentials::Array{BallTreeDensity,1}
-    potentialfac::Vector{Symbol}
-end
-
-"""
-$(TYPEDEF)
-
-TODO TO BE DEPRECATED
-"""
-mutable struct CliqGibbsMC
-    prods::Array{PotProd,1}
-    lbls::Vector{Symbol}
-    CliqGibbsMC() = new()
-    CliqGibbsMC(a,b) = new(a,b)
-end
-
-"""
-$(TYPEDEF)
-
-TODO TO BE DEPRECATED
-"""
-mutable struct DebugCliqMCMC
-  mcmc::Union{Nothing, Array{CliqGibbsMC,1}}
-  outmsg::LikelihoodMessage
-  outmsglbls::Dict{Symbol, Symbol} # Int
-  priorprods::Vector{CliqGibbsMC}
-  DebugCliqMCMC() = new()
-  DebugCliqMCMC(a,b,c,d) = new(a,b,c,d)
-end
 
 
 ##==============================================================================
 ## Deprecate as part of Manifolds.jl consolidation
 ##==============================================================================
 
 
-# FIXME, much consolidation required here
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousScalar})    = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{1}}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{2}}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{3}}) = AMP.Euclid3
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{4}}) = AMP.Euclid4
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = AMP.Circle1
-
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = AMP.Circle1
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{EuclidDistance}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = convert(Manifold, ContinuousEuclid{N})
+# Legacy support
+getManifolds(::InstanceType{Manifolds.Euclidean{Tuple{N}, ℝ}}) where N = tuple([:Euclid for i in 1:N]...)
+getManifolds(::InstanceType{Manifolds.Circle{ℝ}})  = (:Circular,)
 
 
 
 ##==============================================================================
 ## Deprecate code below before v0.23
 ##==============================================================================
 
+
+# FIXME, much consolidation required here
+# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{1}}) = AMP.Euclid
+# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{2}}) = AMP.Euclid2
+# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{3}}) = AMP.Euclid3
+# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{4}}) = AMP.Euclid4
+
+# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = AMP.Circle1
+
+# convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{1}}) = AMP.Euclid
+# convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{2}}) = AMP.Euclid2
+
 export Sphere1
 
-@warn "Deprecating old use of Sphere1, being replaced by Cicular instead"
+@warn "Deprecating old use of Sphere1, being replaced by Circular instead"
 const Sphere1 = Circular
 # @deprecate Sphere1(w...;kw...) Circular(w...;kw...)
 
@@ -96,4 +62,46 @@ const Sphere1 = Circular
 @deprecate PackedSphere1Sphere1(w...;kw...) PackedCircularCircular(w...;kw...)
 
 
+
+# """
+# $(TYPEDEF)
+
+# TODO TO BE DEPRECATED
+# """
+# mutable struct PotProd
+#     Xi::Symbol # Int
+#     prev::Array{Float64,2}
+#     product::Array{Float64,2}
+#     potentials::Array{BallTreeDensity,1}
+#     potentialfac::Vector{Symbol}
+# end
+
+# """
+# $(TYPEDEF)
+
+# TODO TO BE DEPRECATED
+# """
+# mutable struct CliqGibbsMC
+#     prods::Array{PotProd,1}
+#     lbls::Vector{Symbol}
+#     CliqGibbsMC() = new()
+#     CliqGibbsMC(a,b) = new(a,b)
+# end
+
+
+# """
+# $(TYPEDEF)
+
+# TODO TO BE DEPRECATED
+# """
+# mutable struct DebugCliqMCMC
+#   mcmc::Union{Nothing, Array{CliqGibbsMC,1}}
+#   outmsg::LikelihoodMessage
+#   outmsglbls::Dict{Symbol, Symbol} # Int
+#   priorprods::Vector{CliqGibbsMC}
+#   DebugCliqMCMC() = new()
+#   DebugCliqMCMC(a,b,c,d) = new(a,b,c,d)
+# end
+
+
 #

src/FGOSUtils.jl

@@ -81,7 +81,7 @@ end
 # extend convenience function
 function manikde!(pts::AbstractArray{Float64,2},
                   bws::Vector{Float64},
-                  variableType::Union{InstanceType{InferenceVariable}, InstanceType{FunctorInferenceType}}  )
+                  variableType::Union{<:InstanceType{InferenceVariable}, <:InstanceType{FunctorInferenceType}}  )
   #
   addopT, diffopT, getManiMu, getManiLam = buildHybridManifoldCallbacks(getManifolds(variableType))
   bel = KernelDensityEstimate.kde!(pts, bws, addopT, diffopT)
@@ -186,19 +186,14 @@ end
 
 
 
-function calcMean(mkd::ManifoldKernelDensity)
-  error("not implemented yet")
-
-end
-
-
 """
     $SIGNATURES
 
 Get the ParametricPointEstimates---based on full marginal belief estimates---of a variable in the distributed factor graph.
 
 DevNotes
 - TODO update for manifold subgroups.
+- TODO standardize after AMP3D
 
 Related
 
@@ -210,24 +205,26 @@ function calcPPE( var::DFGVariable,
                   solveKey::Symbol=:default  )
   #
   P = getBelief(var, solveKey)
-  manis = getManifolds(varType) # getManifolds(vnd)
+  maniDef = convert(Manifold, varType)
+  manis = getManifolds(maniDef) # varType # getManifolds(vnd)
   ops = buildHybridManifoldCallbacks(manis)
-  Pme = getKDEMean(P) #, addop=ops[1], diffop=ops[2]
+  Pme = calcMean(P)  # getKDEMean(P) #, addop=ops[1], diffop=ops[2]
   Pma = getKDEMax(P, addop=ops[1], diffop=ops[2])
-  suggested = zeros(getDimension(var))
-  # TODO standardize after AMP3D
-  @assert length(manis) == getDimension(var)
-  for i in 1:length(manis)
-    mani = manis[i]
-    if mani == :Euclid
-      suggested[i] = Pme[i]
-    elseif mani == :Circular
-      suggested[i] = Pma[i]
-    else
-      error("Unknown manifold to find PPE, $varType, $mani")
-    end
-  end
-  MeanMaxPPE(solveKey, suggested, Pma, Pme, now())
+  # suggested = zeros(getDimension(var))
+  # @assert length(manis) == getDimension(var)
+  # for i in 1:length(manis)
+  #   mani = manis[i]
+  #   if mani == :Euclid
+  #     suggested[i] = Pme[i]
+  #   elseif mani == :Circular
+  #     suggested[i] = Pma[i]
+  #   else
+  #     error("Unknown manifold to find PPE, $varType, $mani")
+  #   end
+  # end
+
+  # suggested, max, mean, current time
+  MeanMaxPPE(solveKey, Pme, Pma, Pme, now())
 end
 
 

src/Factors/Circular.jl

@@ -19,7 +19,6 @@ end
 const Sphere1Sphere1 = CircularCircular
 
 CircularCircular(::UniformScaling) = CircularCircular(Normal())
-# Sphere1Sphere1()
 
 
 getSample(cf::CalcFactor{<:CircularCircular}, N::Int=1) = (reshape(rand(cf.factor.Z,N),:,N), )
@@ -35,6 +34,9 @@ function (cf::CalcFactor{<:CircularCircular})(meas,
 end
 
 
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = Manifolds.Circle()
+
+
 """
 $(TYPEDEF)
 
@@ -62,6 +64,7 @@ function getSample(cf::CalcFactor{<:PriorCircular}, N::Int=1)
 end
 
 
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{PriorCircular}) = Manifolds.Circle()
 
 
 
@@ -72,8 +75,8 @@ Serialized object for storing PriorCircular.
 """
 mutable struct PackedPriorCircular  <: IncrementalInference.PackedInferenceType
   datastr::String
-  PackedPriorCircular() = new()
-  PackedPriorCircular(x::String) = new(x)
+  # PackedPriorCircular() = new()
+  # PackedPriorCircular(x::String) = new(x)
 end
 function convert(::Type{PackedPriorCircular}, d::PriorCircular)
   return PackedPriorCircular(convert(PackedSamplableBelief, d.Z))
@@ -97,8 +100,8 @@ Serialized object for storing CircularCircular.
 """
 mutable struct PackedCircularCircular  <: IncrementalInference.PackedInferenceType
   datastr::String
-  PackedCircularCircular() = new()
-  PackedCircularCircular(x::String) = new(x)
+  # PackedCircularCircular() = new()
+  # PackedCircularCircular(x::String) = new(x)
 end
 function convert(::Type{CircularCircular}, d::PackedCircularCircular)
   return CircularCircular(convert(SamplableBelief, d.datastr))

src/Factors/EuclidDistance.jl

@@ -31,6 +31,7 @@ function (s::CalcFactor{<:EuclidDistance})(z, x1, x2)
 end
 
 
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{EuclidDistance}) = Manifolds.Euclidean(1)
 
 
 """

src/Factors/LinearRelative.jl

@@ -48,8 +48,8 @@ end
 
 
 
-convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{1}}) = AMP.Euclid
-convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{2}}) = AMP.Euclid2
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.Euclidean(N) 
+# convert(Manifold, ContinuousEuclid{N})
 
 
 
@@ -59,8 +59,8 @@ Serialization type for `LinearRelative` binary factor.
 """
 mutable struct PackedLinearRelative <: PackedInferenceType
   Z::String
-  PackedLinearRelative() = new()
-  PackedLinearRelative(z::AS) where {AS <: AbstractString} = new(z)
+  # PackedLinearRelative() = new()
+  # PackedLinearRelative(z::AS) where {AS <: AbstractString} = new(z)
 end
 function convert(::Type{PackedLinearRelative}, d::LinearRelative)
   PackedLinearRelative(convert(PackedSamplableBelief, d.Z))

src/IncrementalInference.jl

@@ -12,6 +12,10 @@ using Reexport
 # @reexport using Graphs
 @reexport using LinearAlgebra
 
+using Manifolds
+
+export ℝ, Manifold, Euclidean, Circle
+
 import NLsolve
 import NLSolversBase
 import Optim