mkd to Manifold objects, AMP v0.3.1

Project.toml

@@ -41,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"

src/Deprecated.jl

@@ -34,7 +34,6 @@ getManifolds(::InstanceType{Manifolds.Circle{ℝ}})  = (:Circular,)
 
 
 
-
 ##==============================================================================
 ## Deprecate code below before v0.23
 ##==============================================================================
@@ -53,7 +52,7 @@ getManifolds(::InstanceType{Manifolds.Circle{ℝ}})  = (:Circular,)
 
 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...)
 

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

@@ -34,7 +34,7 @@ function (cf::CalcFactor{<:CircularCircular})(meas,
 end
 
 
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = Manifolds.Circle{ℝ}
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = Manifolds.Circle()
 
 
 """
@@ -64,7 +64,7 @@ function getSample(cf::CalcFactor{<:PriorCircular}, N::Int=1)
 end
 
 
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{PriorCircular}) = Manifolds.Circle{ℝ}
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{PriorCircular}) = Manifolds.Circle()
 
 
 

src/Factors/EuclidDistance.jl

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

src/Factors/LinearRelative.jl

@@ -48,7 +48,7 @@ end
 
 
 
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.Euclidean{Tuple{N}, ℝ} 
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.Euclidean(N) 
 # convert(Manifold, ContinuousEuclid{N})
 
 

src/IncrementalInference.jl

@@ -14,6 +14,8 @@ using Reexport
 
 using Manifolds
 
+export ℝ, Manifold, Euclidean, Circle
+
 import NLsolve
 import NLSolversBase
 import Optim

src/JunctionTree.jl

@@ -381,39 +381,39 @@ Kaess et al.: iSAM2, IJRR, 2011, [Alg. 3]
 Fourie, D.: mmisam, PhD thesis, 2017. [Chpt. 5]
 """
 function newPotential(tree::AbstractBayesTree, dfg::G, var::Symbol, elimorder::Array{Symbol,1}) where G <: AbstractDFG
-    firvert = DFG.getVariable(dfg,var)
-    # no parent
-    if (length(getSolverData(firvert).separator) == 0)
-      # if (length(getCliques(tree)) == 0)
-        # create new root
-        addClique!(tree, dfg, var)
-      # else
-      #   # add to root
-      #   @warn "root append clique is happening"
-      #   appendClique!(tree, 1, dfg, var)
-      # end
+  firvert = DFG.getVariable(dfg,var)
+  # no parent
+  if (length(getSolverData(firvert).separator) == 0)
+    # if (length(getCliques(tree)) == 0)
+      # create new root
+      addClique!(tree, dfg, var)
+    # else
+    #   # add to root
+    #   @warn "root append clique is happening"
+    #   appendClique!(tree, 1, dfg, var)
+    # end
+  else
+    # find parent clique Cp that containts the first eliminated variable of Sj as frontal
+    Sj = getSolverData(firvert).separator
+    felbl = identifyFirstEliminatedSeparator(dfg, elimorder, firvert, Sj).label
+    # get clique id of first eliminated frontal
+    CpID = tree.frontals[felbl]
+    # look to add this conditional to the tree
+    cliq = getClique(tree, CpID)
+    # clique of the first eliminated frontal
+    unFC = union(getCliqFrontalVarIds(cliq), getCliqSeparatorVarIds(cliq))
+    # if the separator of this new variable is identical to the (entire) clique of the firstly eliminated frontal.
+    if (sort(unFC) == sort(Sj))
+      # just add new variable as frontal to this clique
+      # insert conditional (p(var|sepr)) into clique CpID -- i.e. just adding a frontal
+      # @info "adding new frontal $var to existing clique $CpID"
+      appendClique!(tree, CpID, dfg, var)
     else
-      # find parent clique Cp that containts the first eliminated variable of Sj as frontal
-      Sj = getSolverData(firvert).separator
-      felbl = identifyFirstEliminatedSeparator(dfg, elimorder, firvert, Sj).label
-      # get clique id of first eliminated frontal
-      CpID = tree.frontals[felbl]
-      # look to add this conditional to the tree
-      cliq = getClique(tree, CpID)
-      # clique of the first eliminated frontal
-      unFC = union(getCliqFrontalVarIds(cliq), getCliqSeparatorVarIds(cliq))
-      # if the separator of this new variable is identical to the (entire) clique of the firstly eliminated frontal.
-      if (sort(unFC) == sort(Sj))
-        # just add new variable as frontal to this clique
-        # insert conditional (p(var|sepr)) into clique CpID -- i.e. just adding a frontal
-        # @info "adding new frontal $var to existing clique $CpID"
-        appendClique!(tree, CpID, dfg, var)
-      else
-        # a new child clique is required here (this becomes parent)
-        # @info "adding new child clique with parent separator."
-        newChildClique!(tree, dfg, CpID, var, Sj)
-      end
+      # a new child clique is required here (this becomes parent)
+      # @info "adding new child clique with parent separator."
+      newChildClique!(tree, dfg, CpID, var, Sj)
     end
+  end
 end
 
 """
@@ -1260,6 +1260,10 @@ function compCliqAssocMatrices!(dfg::G, bt::AbstractBayesTree, cliq::TreeClique)
   cols = [frtl;cond]
   getCliqueData(cliq).inmsgIDs = inmsgIDs
   getCliqueData(cliq).potIDs = potIDs
+
+  @debug "Building cliqAssocMat" cliq
+  @debug "Building cliqAssocMat" cliq.id string(inmsgIDs) string(potIDs) 
+
   cliqAssocMat = Array{Bool,2}(undef, length(potIDs), length(cols))
   cliqMsgMat = Array{Bool,2}(undef, length(inmsgIDs), length(cols))
   fill!(cliqAssocMat, false)
@@ -1284,6 +1288,7 @@ function compCliqAssocMatrices!(dfg::G, bt::AbstractBayesTree, cliq::TreeClique)
       end
     end
   end
+  @debug "Final cliqAssocMat" cliq.id cliqAssocMat
   getCliqueData(cliq).cliqAssocMat = cliqAssocMat
   getCliqueData(cliq).cliqMsgMat = cliqMsgMat
   nothing

src/Variables/Circular.jl

@@ -1,5 +1,5 @@
 
-export Circular
+export Circular, Circle
 
 """
 $(TYPEDEF)
@@ -9,6 +9,6 @@ Sphere1 is a S1 mechanization of one Circular rotation, with `theta in [-pi,pi)`
 @defVariable Circular 1 (:Circular,)
 
 
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = Manifolds.Circle{ℝ}
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = Manifolds.Circle()
 
 #

src/Variables/DefaultVariables.jl

@@ -44,6 +44,6 @@ getDimension(::ContinuousEuclid{N}) where N = N::Int
 getManifolds(::ContinuousEuclid{N}) where N = ntuple(i -> :Euclid, N)
 
 
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{N}}) where N = Manifolds.Euclidean{Tuple{N}, ℝ}
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousScalar})    = Manifolds.Euclidean{Tuple{1}, ℝ} # AMP.Euclid
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{N}}) where N = Manifolds.Euclidean(N) # {Tuple{N}, ℝ}
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousScalar})    = Manifolds.Euclidean(1)         # {Tuple{1}, ℝ} # AMP.Euclid
 

test/testCliqSolveDbgUtils.jl

@@ -1,8 +1,12 @@
 using IncrementalInference
 using Test
 
+##
+
 @testset "Test solveCliqueUp! and solveCliqDown!" begin
+
 ##
+
 N=8
 fg = generateCanonicalFG_lineStep(N; 
                                   graphinit=false,
@@ -11,9 +15,11 @@ fg = generateCanonicalFG_lineStep(N;
                                   posePriorsAt=[0],
                                   landmarkPriorsAt=[], 
                                   sightDistance=N+1)
+#
 
 deleteFactor!.(fg, [Symbol("x$(i)lm0f1") for i=1:(N-1)])
 
+# test the ensureAllInitialized! separately anyway
 ensureAllInitialized!(fg)
 
 tree = buildTreeReset!(fg)
@@ -30,7 +36,9 @@ a,b = solveCliqUp!(fg, tree, 2; recordcliq = true)
 a,b = solveCliqDown!(fg, tree, 2)
 a,b = solveCliqDown!(fg, tree, 2; recordcliq = true) 
 @test length(a) > 0
+
 ##
+
 end
 
 

test/testMixtureLinearConditional.jl

@@ -3,6 +3,7 @@
 
 using IncrementalInference
 using Test
+# using Manifolds # should be done within regular exports
 
 ##
 

test/testSphere1.jl

@@ -4,16 +4,16 @@ using Test
 
 ##
 
-@testset "test Sphere1D" begin
+@testset "test Circular" begin
 
 ##
 
 fg = initfg()
 getSolverParams(fg).useMsgLikelihoods = true
 
-addVariable!.(fg, [Symbol("x$i") for i=0:4], Sphere1)
-addFactor!(fg, [:x0], PriorSphere1(Normal(0.0,0.1)))
-map(i->addFactor!(fg, [Symbol("x$i"),Symbol("x$(i+1)")], Sphere1Sphere1(Normal(1.0, 0.1))), 0:3)
+addVariable!.(fg, [Symbol("x$i") for i=0:4], Circular)
+addFactor!(fg, [:x0], PriorCircular(Normal(0.0,0.1)))
+map(i->addFactor!(fg, [Symbol("x$i"),Symbol("x$(i+1)")], CircularCircular(Normal(1.0, 0.1))), 0:3)
 
 
 solveTree!(fg);