Fix some sampling dispatch and cleanup (#1880)

Author: Affie

IncrementalInference/src/Deprecated.jl

@@ -35,13 +35,27 @@ function _perturbIfNecessary(
 end
 #
 
+# lets create all the vertices first and then deal with the elimination variables thereafter
+function addBayesNetVerts!(dfg::AbstractDFG, elimOrder::Array{Symbol, 1})
+  #
+  for pId in elimOrder
+    vert = DFG.getVariable(dfg, pId)
+    if  getState(vert, :default).BayesNetVertID === nothing ||
+        getState(vert, :default).BayesNetVertID == :_null # Special serialization case of nothing
+      @debug "[AddBayesNetVerts] Assigning $pId.data.BayesNetVertID = $pId"
+      getState(vert, :default).BayesNetVertID = pId
+    else
+      @warn "addBayesNetVerts -- Something is wrong, variable '$pId' should not have an existing Bayes net reference to '$(getState(vert, :default).BayesNetVertID)'"
+    end
+  end
+end
 
 ## ================================================================================================
 ## ================================================================================================
 
 # TODO maybe upstream to DFG
-DFG.MeanMaxPPE(solveKey::Symbol, suggested::SVector, max::SVector, mean::SVector) =
-  DFG.MeanMaxPPE(solveKey, collect(suggested), collect(max), collect(mean))
+DFG.MeanMaxPPE(solveKey::Symbol, suggested::StaticArray, max::StaticArray, mean::StaticArray) =
+  DFG.MeanMaxPPE(solveKey, Vector(suggested), Vector(max), Vector(mean))
 
 
 ## ================================================================================================

IncrementalInference/src/Factors/Circular.jl

@@ -13,9 +13,10 @@ function getSample(cf::CalcFactor{<:CircularCircular})
   return sampleTangent(getManifold(cf), cf.factor.Z, getPointIdentity(Circular))
 end
 
-function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{CircularCircular})
-  return Manifolds.RealCircleGroup()
-end
+#TODO should be deprecated?
+# function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{CircularCircular})
+#   return Manifolds.RealCircleGroup()
+# end
 
 IIFTypes.CircularCircular(::UniformScaling) = CircularCircular(Normal())
 
@@ -40,8 +41,9 @@ function (cf::CalcFactor{<:PriorCircular})(m, p)
   return Xc
 end
 
-function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{PriorCircular})
-  return Manifolds.RealCircleGroup()
-end
+#TODO should be deprecated?
+# function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{PriorCircular})
+#   return Manifolds.RealCircleGroup()
+# end
 
 # --------------------------------------------

IncrementalInference/src/Factors/EuclidDistance.jl

@@ -19,9 +19,10 @@ getDimension(::InstanceType{<:EuclidDistance}) = 1
 # new and simplified interface for both nonparametric and parametric
 (s::CalcFactor{<:EuclidDistance})(z, x1, x2) = z .- norm(x2 .- x1)
 
-function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{EuclidDistance})
-  return LieGroups.TranslationGroup(1)
-end
+#TODO should be deprecated?
+# function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{EuclidDistance})
+#   return LieGroups.TranslationGroup(1)
+# end
 
 """
 $(TYPEDEF)

IncrementalInference/src/manifolds/services/ManifoldSampling.jl

@@ -9,61 +9,34 @@ Notes
 """
 function sampleTangent end
 
-# Sampling MKD
-# function sampleTangent(M::AbstractDecoratorManifold, x::ManifoldKernelDensity, p = mean(x))
-#   # get legacy matrix of coordinates and selected labels
-#   #TODO make sure that when `sample` is replaced in MKD, coordinates is a vector
-#   coords, lbls = sample(x.belief, 1)
-#   X = hat(x.manifold, p, coords[:])
-#   return X
-# end
-
 function sampleTangent(x::ManifoldKernelDensity, p = mean(x))
-  Base.depwarn(
-    "sampleTangent(x::ManifoldKernelDensity, p) should be replaced by sampleTangent(M<:AbstractManifold, x::ManifoldKernelDensity, p)",
-    :sampleTangent,
-  )
-  return sampleTangent(x.manifold, x, p)
+  error("sampleTangent(x::ManifoldKernelDensity, p) should be replaced by sampleTangent(M<:AbstractManifold, x::ManifoldKernelDensity, p)")
 end
 
 # Sampling Distributions
 # assumes M is a group and will break for Riemannian, but leaving that enhancement as TODO
 function sampleTangent(
   M::AbstractManifold,
-  z::Distribution,
-  p = getPointIdentity(M),
+  z,
+  p,
   basis::AbstractBasis = DefaultOrthogonalBasis(),
 )
   return get_vector(M, p, rand(z), basis)
 end
 
-# function sampleTangent(
-#   M::AbstractDecoratorManifold,
-#   z::Distribution,
-#   p = getPointIdentity(M),
-# )
-#   return hat(M, p, SVector{length(z)}(rand(z))) #TODO make sure all Distribution has length, 
-#   # if this errors maybe fall back no next line
-#   # return convert(typeof(p), hat(M, p, rand(z, 1)[:])) #TODO find something better than (z,1)[:]
-# end
-
-function sampleTangent(M::AbstractLieGroup, z::Distribution, p = getPointIdentity(M))
-  return hat(LieAlgebra(M), SVector{length(z)}(rand(z)), typeof(p))
+function sampleTangent(M::AbstractLieGroup, z, p = getPointIdentity(M))
+  return hat(LieAlgebra(M), SVector{manifold_dimension(M)}(rand(z)), typeof(p))
 end
 
 function sampleTangent(M::typeof(LieGroups.CircleGroup()), z::Distribution, p = getPointIdentity(M))
   return hat(LieAlgebra(M), rand(z))
 end
 
-function sampleTangent(M::AbstractLieGroup, z, p = getPointIdentity(M))
-  return hat(LieAlgebra(M), rand(z), typeof(p))
-end
-
 function sampleTangent(M::AbstractLieGroup, x::ManifoldKernelDensity, p = mean(x))
   # get legacy matrix of coordinates and selected labels
   #TODO make sure that when `sample` is replaced in MKD, coordinates is a vector
   coords, lbls = sample(x.belief, 1)
-  X = hat(LieAlgebra(x.manifold), coords[:], typeof(p))
+  X = hat(LieAlgebra(M), coords[:], typeof(p))
   return X
 end
 

IncrementalInference/src/manifolds/services/ManifoldsExtentions.jl

@@ -123,7 +123,7 @@ end
 
 #TODO test
 function DFG.getPointIdentity(
-  PrG::AbstractLieGroup{𝔽, Op, M},
+  PrG::LieGroup{𝔽, Op, M},
   ::Type{T} = Float64,
 ) where {𝔽, Op <: AbstractProductGroupOperation, M <: ProductManifold, T <: Real}
   PrM = PrG.manifold
@@ -162,7 +162,7 @@ end
 function DFG.getPointIdentity(
   ::typeof(SpecialEuclideanGroup(3; variant = :right)),
   ::Type{T} = Float64,
-) where {T}
+) where {T <: Real}
   N = 3
   return ArrayPartition(zeros(SVector{N, T}), SMatrix{N, N, T}(I))
 end

IncrementalInference/src/services/BayesNet.jl

@@ -71,21 +71,6 @@ function getEliminationOrder(
   return permuteds[p]
 end
 
-# lets create all the vertices first and then deal with the elimination variables thereafter
-function addBayesNetVerts!(dfg::AbstractDFG, elimOrder::Array{Symbol, 1})
-  #
-  for pId in elimOrder
-    vert = DFG.getVariable(dfg, pId)
-    if  getState(vert, :default).BayesNetVertID === nothing ||
-        getState(vert, :default).BayesNetVertID == :_null # Special serialization case of nothing
-      @debug "[AddBayesNetVerts] Assigning $pId.data.BayesNetVertID = $pId"
-      getState(vert, :default).BayesNetVertID = pId
-    else
-      @warn "addBayesNetVerts -- Something is wrong, variable '$pId' should not have an existing Bayes net reference to '$(getState(vert, :default).BayesNetVertID)'"
-    end
-  end
-end
-
 function addConditional!(dfg::AbstractDFG, vertId::Symbol, Si::Vector{Symbol})
   #
   bnv = DFG.getVariable(dfg, vertId)

IncrementalInference/src/services/TreeMessageUtils.jl

@@ -311,7 +311,7 @@ function addLikelihoodsDifferentialCHILD!(
       # chain of user factors are of the same type
       if isHom
         _sft = selectFactorType(tfg, sym1_, sym2_)
-        sft = _sft() #FIXME empty factor observation constructor
+        sft = _sft(MvNormal( getDimension(getManifold(_sft)), 1.0)) #FIXME empty factor observation constructor
         # only take factors that are homogeneous with the generic relative
         if typeof(sft).name == ftyps[1]
           # assume default helper function # buildFactorDefault(nfactype)

IncrementalInference/test/manifolds/factordiff.jl

@@ -51,7 +51,7 @@ end
 
 
 # ## 
-# @testset "using RoME; FiniteDiff.jacobian of SpecialEuclidean(2) factor" begin
+# @testset "using RoME; FiniteDiff.jacobian of SpecialEuclideanGroup(2) factor" begin
 # ##
 
 # fg = LocalDFG(;

IncrementalInference/test/runtests.jl

@@ -4,16 +4,19 @@ using Test
 using LieGroups: TranslationGroup
 
 TEST_GROUP = get(ENV, "IIF_TEST_GROUP", "all")
-
+@testset "IncrementalInference Tests" begin
 # temporarily moved to start (for debugging)
 #...
 if TEST_GROUP in ["all", "tmp_debug_group"]
+@testset "Temporary Debug Group" begin
 include("testSpecialOrthogonalMani.jl")
 include("testMultiHypo3Door.jl")
 include("priorusetest.jl")
 end
+end
 
 if TEST_GROUP in ["all", "basic_functional_group"]
+@testset "Basic Functional Group" begin
 # more frequent stochasic failures from numerics
 include("testSpecialEuclidean2Mani.jl")
 include("testEuclidDistance.jl")
@@ -31,7 +34,7 @@ include("testCliqSolveDbgUtils.jl")
 include("basicGraphsOperations.jl")
 
 # regular testing
-@test_broken include("testSphereMani.jl")
+@test_broken error("testSphereMani.jl broken")#include("testSphereMani.jl")
 include("testBasicManifolds.jl")
 include("testDERelative.jl")
 include("testHeatmapGridDensity.jl")
@@ -86,8 +89,10 @@ include("testSolveOrphanedFG.jl")
 include("testSolveSetPPE.jl")
 include("testSolveKey.jl")
 end
+end
 
 if TEST_GROUP in ["all", "test_cases_group"]
+@testset "Test Cases Group" begin
 include("testnullhypothesis.jl") 
 include("testVariousNSolveSize.jl")
 include("testExplicitMultihypo.jl")
@@ -114,6 +119,7 @@ end
 # include("testMultiprocess.jl")
 include("testDeadReckoningTether.jl")
 end
-
+end
+end
 
 #

IncrementalInference/test/testSpecialEuclidean2Mani.jl

@@ -46,7 +46,7 @@ end
 
 ##
 
-@testset "Test SpecialEuclidean(2)" begin
+@testset "Test SpecialEuclideanGroup(2)" begin
 ##
 
 M = getManifold(SpecialEuclidean2)
@@ -66,8 +66,8 @@ fg = initfg()
 
 v0 = addVariable!(fg, :x0, SpecialEuclidean2)
 
-# mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
-# mp = ManifoldPrior(SpecialEuclidean(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal(Diagonal(abs2.([0.01, 0.01, 0.01]))))
+# mp = ManifoldPrior(SpecialEuclideanGroup(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
+# mp = ManifoldPrior(SpecialEuclideanGroup(2), ArrayPartition(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal(Diagonal(abs2.([0.01, 0.01, 0.01]))))
 # mp = ManifoldPrior(SE2, ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.]), MvNormal(Diagonal(abs2.([0.01, 0.01, 0.01]))))
 mp = ManifoldPrior(SE2, ArrayPartition(SA[0.0,0.0], SA[1.0 0.0; 0.0 1.]), MvNormal(Diagonal(abs2.(SA[0.01, 0.01, 0.01]))))
 p = addFactor!(fg, [:x0], mp)