JuliaRobotics · dehann · Jul 8, 2021 · Jul 8, 2021 · Jul 8, 2021 · Jul 8, 2021
diff --git a/src/ApproxConv.jl b/src/ApproxConv.jl
@@ -252,7 +252,7 @@ function calcVariableDistanceExpectedFractional(ccwl::CommonConvWrapper,
 end
 
 # Add entrypy on a point in `points` on manifold M, only on dimIdx if in p 
-function addEntropyOnManifoldHack!( M::ManifoldsBase.AbstractManifold,
+function addEntropyOnManifold!( M::ManifoldsBase.AbstractManifold,
                                     points::Union{<:AbstractVector{<:Real},SubArray}, 
                                     dimIdx::AbstractVector, 
                                     spreadDist::Real,
@@ -281,16 +281,6 @@ function addEntropyOnManifoldHack!( M::ManifoldsBase.AbstractManifold,
 
   end
   #
-  # manis = convert(Tuple, M) # LEGACY, TODO REMOVE
-  # # TODO deprecate
-  # maniAddOps, _, _, _ = buildHybridManifoldCallbacks(manis)
-  # # add 1σ "noise" level to max distance as control
-  # # 1:size(addEntr, 1)
-  # for dim in dimIdx, idx in 1:length(addEntr)
-  #   if (p === :) || dim in p
-  #     addEntr[idx][dim] = maniAddOps[dim](addEntr[idx][dim], spreadDist*(rand()-0.5))
-  #   end
-  # end
   nothing
 end
 
@@ -333,7 +323,7 @@ function computeAcrossHypothesis!(ccwl::Union{<:CommonConvWrapper{F},
       # consider duplicate convolution approximations for inflation off-zero
       # ultimately set by dfg.params.inflateCycles
       for iflc in 1:inflateCycles
-        addEntropyOnManifoldHack!(mani, addEntr, 1:getDimension(mani), spreadDist, cpt_.p)
+        addEntropyOnManifold!(mani, addEntr, 1:getDimension(mani), spreadDist, cpt_.p)
         # no calculate new proposal belief on kernels `allelements[count]`
         skipSolve ? @warn("skipping numerical solve operation") : approxConvOnElements!(ccwl, allelements[count])
       end
@@ -348,7 +338,7 @@ function computeAcrossHypothesis!(ccwl::Union{<:CommonConvWrapper{F},
         addEntr = view(ccwl.params[sfidx], allelements[count])
         # dynamic estimate with user requested speadNH of how much noise to inject (inflation or nullhypo)
         spreadDist = calcVariableDistanceExpectedFractional(ccwl, sfidx, certainidx, kappa=spreadNH)
-        addEntropyOnManifoldHack!(mani, addEntr, 1:getDimension(mani), spreadDist)
+        addEntropyOnManifold!(mani, addEntr, 1:getDimension(mani), spreadDist)
 
     elseif hypoidx == 0
       # basically do nothing since the factor is not active for these allelements[count]
@@ -358,7 +348,7 @@ function computeAcrossHypothesis!(ccwl::Union{<:CommonConvWrapper{F},
       # dynamic estimate with user requested speadNH of how much noise to inject (inflation or nullhypo)
       spreadDist = calcVariableDistanceExpectedFractional(ccwl, sfidx, certainidx, kappa=spreadNH)
       # # make spread (1σ) equal to mean distance of other fractionals
-      addEntropyOnManifoldHack!(mani, addEntr, 1:getDimension(mani), spreadDist)
+      addEntropyOnManifold!(mani, addEntr, 1:getDimension(mani), spreadDist)
     else
       error("computeAcrossHypothesis -- not dealing with multi-hypothesis case correctly")
     end
@@ -518,7 +508,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
         addEntr[m] .= ccwl.measurement[1][m]
       end
       # ongoing part of RoME.jl #244
-      addEntropyOnManifoldHack!(mani, addEntrNH, 1:getDimension(mani), spreadDist)
+      addEntropyOnManifold!(mani, addEntrNH, 1:getDimension(mani), spreadDist)
   else
     i = 0
     for dimnum in fnc.partial
@@ -529,7 +519,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
       # @show size(addEntr), dimnum, nhmask
       addEntrNHp = view(view(addEntr, (1:length(ahmask))[ahmask]), dimnum)
       # ongoing part of RoME.jl #244
-      addEntropyOnManifoldHack!(mani, addEntrNHp, dimnum:dimnum, spreadDist)
+      addEntropyOnManifold!(mani, addEntrNHp, dimnum:dimnum, spreadDist)
     end
   end
   return addEntr

diff --git a/src/CompareUtils.jl b/src/CompareUtils.jl
@@ -25,7 +25,7 @@ function compareAllSpecial( A::T1, B::T2;
                             skip=Symbol[], show::Bool=true) where {T1 <: CommonConvWrapper, T2 <: CommonConvWrapper}
   #
   if T1 != T2
-    @warn "CCW types not equal" T1 T2
+    @warn "CCW types T1 and T2 not equal=>" T1 T2
     return false
   end
   # FIXME still issues with compare, skipping :vartypes https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/434

diff --git a/src/DispatchPackedConversions.jl b/src/DispatchPackedConversions.jl
@@ -52,10 +52,12 @@ function convert(
   # reconstitute from stored data
   # FIXME, add threadmodel=threadmodel
   # FIXME https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/590#issuecomment-776838053
+  # FIXME dont know what manifolds to use in ccw
   ccw = prepgenericconvolution(DFG.DFGVariable[], usrfnc, multihypo=mhcat, nullhypo=nh, inflation=packed.inflation)
   ccw.certainhypo = packed.certainhypo
 
-  ret = FunctionNodeData{CommonConvWrapper{typeof(usrfnc)}}(packed.eliminated, packed.potentialused, packed.edgeIDs, ccw,
+  # CommonConvWrapper{typeof(usrfnc)}
+  ret = FunctionNodeData{typeof(ccw)}(packed.eliminated, packed.potentialused, packed.edgeIDs, ccw,
                                                             packed.multihypo, packed.certainhypo, packed.nullhypo, 
                                                             packed.solveInProgress, packed.inflation )
   #

diff --git a/src/FGOSUtils.jl b/src/FGOSUtils.jl
@@ -140,19 +140,7 @@ function manikde!(pts::AbstractVector{P},
   return AMP.manikde!(M, pts)
 end
 
-# manikde!( pts::AbstractVector{<:Real}, 
-#           vartype::Type{<:ContinuousScalar}) = manikde!(reshape(pts,1,:), vartype)
-#
 
-# TEMPORARY legacy wrapper
-# function manikde!(ptsArr::Vector{Vector{Float64}}, 
-#                   bw::Vector{Float64}, 
-#                   varType::Union{InstanceType{<:InferenceVariable}, InstanceType{<:AbstractFactor}} )
-#   #
-#   arr = Matrix{Float64}(undef, length(ptsArr[1]), length(ptsArr))
-#   @cast arr[i,j] = ptsArr[j][i]
-#   manikde!( arr, bw, varType )  
-# end
 
 """
     $SIGNATURES

diff --git a/src/FactorGraph.jl b/src/FactorGraph.jl
@@ -594,20 +594,21 @@ Notes
 - Will not work in all situations, but good enough so far.
   - # TODO standardize via domain or manifold definition...??
 """
-function calcZDim(cf::CalcFactor{T}) where {T <: FunctorInferenceType}
+function calcZDim(cf::CalcFactor{T}) where {T <: AbstractFactor}
   #
-  # zdim = T != GenericMarginal ? size(getSample(usrfnc, 2)[1],1) : 0
-  zdim = if T != GenericMarginal
-    # vnds = Xi # (x->getSolverData(x)).(Xi)
-    # NOTE try to make sure we get matrix back (not a vector)
-    smpls = sampleFactor(cf, 2)[1]
-    length(smpls[1])
-  else
-    0
-  end
-  return zdim
+
+  # NOTE try to make sure we get matrix back (not a vector)
+  smpls = sampleFactor(cf, 2)[1]
+  return length(smpls[1])
 end
 
+# FIXME THIS IS NEW REPLACEMENT FUNCTION
+# function calcZDim(cf::CalcFactor{T}) where T <: AbstractFactor
+#   return manifold_dimension(getManifold(cf.factor))
+# end
+
+calcZDim(cf::CalcFactor{<:GenericMarginal}) = 0
+
 calcZDim(cf::CalcFactor{<:ManifoldPrior}) = manifold_dimension(cf.factor.M)
 
 
@@ -689,7 +690,7 @@ function getDefaultFactorData(dfg::AbstractDFG,
   ccw = prepgenericconvolution(Xi, usrfnc, multihypo=mhcat, nullhypo=nh, threadmodel=threadmodel, inflation=inflation)
 
   # and the factor data itself
-  return FunctionNodeData{CommonConvWrapper{T}}(eliminated, potentialused, edgeIDs, ccw, multihypo, ccw.certainhypo, nullhypo, solveInProgress, inflation)
+  return FunctionNodeData{typeof(ccw)}(eliminated, potentialused, edgeIDs, ccw, multihypo, ccw.certainhypo, nullhypo, solveInProgress, inflation)
 end
 
 
@@ -1024,10 +1025,10 @@ Related
 initManual!, graphinit (keyword)
 """
 function resetInitialValues!(dest::AbstractDFG,
-                             src::AbstractDFG=dest,
-                             initKey::Symbol=:graphinit,
-                             solveKey::Symbol=:default;
-                             varList::AbstractVector{Symbol}=ls(dest))
+                            src::AbstractDFG=dest,
+                            initKey::Symbol=:graphinit,
+                            solveKey::Symbol=:default;
+                            varList::AbstractVector{Symbol}=ls(dest))
   #
   for vs in varList
     vnd = getSolverData(getVariable(src, vs), initKey)
@@ -1145,24 +1146,23 @@ variables are related to data association uncertainty.
 Experimental
 - `inflation`, to better disperse kernels before convolution solve, see IIF #1051.
 """
-function addFactor!(dfg::AbstractDFG,
-                    Xi::Vector{<:DFGVariable},
-                    usrfnc::R;
-                    multihypo::Vector{Float64}=Float64[],
-                    nullhypo::Float64=0.0,
-                    solvable::Int=1,
-                    tags::Vector{Symbol}=Symbol[],
-                    timestamp::Union{DateTime,ZonedDateTime}=now(localzone()),
-                    graphinit::Bool=getSolverParams(dfg).graphinit,
-                    threadmodel=SingleThreaded,
-                    suppressChecks::Bool=false,
-                    inflation::Real=getSolverParams(dfg).inflation  ) where
-                      {R <: FunctorInferenceType}
+function DFG.addFactor!(dfg::AbstractDFG,
+                        Xi::Vector{<:DFGVariable},
+                        usrfnc::AbstractFactor;
+                        multihypo::Vector{Float64}=Float64[],
+                        nullhypo::Float64=0.0,
+                        solvable::Int=1,
+                        tags::Vector{Symbol}=Symbol[],
+                        timestamp::Union{DateTime,ZonedDateTime}=now(localzone()),
+                        graphinit::Bool=getSolverParams(dfg).graphinit,
+                        threadmodel=SingleThreaded,
+                        suppressChecks::Bool=false,
+                        inflation::Real=getSolverParams(dfg).inflation,
+                        namestring::Symbol = assembleFactorName(dfg, Xi)  )
   #
   # depcrecation
 
   varOrderLabels = Symbol[v.label for v=Xi]
-  namestring = assembleFactorName(dfg, Xi)
   solverData = getDefaultFactorData(dfg, 
                                     Xi, 
                                     deepcopy(usrfnc), 
@@ -1188,16 +1188,17 @@ end
 
 function DFG.addFactor!(dfg::AbstractDFG,
                         xisyms::Vector{Symbol},
-                        usrfnc::FunctorInferenceType;
-                        multihypo::Vector{<:Real}=Float64[],
-                        nullhypo::Float64=0.0,
-                        solvable::Int=1,
-                        timestamp::Union{DateTime,ZonedDateTime}=now(localzone()),
-                        tags::Vector{Symbol}=Symbol[],
-                        graphinit::Bool=getSolverParams(dfg).graphinit,
-                        threadmodel=SingleThreaded,
-                        inflation::Real=getSolverParams(dfg).inflation,
-                        suppressChecks::Bool=false  )
+                        usrfnc::AbstractFactor;
+                        suppressChecks::Bool=false,
+                        kw...  )
+                        # multihypo::Vector{<:Real}=Float64[],
+                        # nullhypo::Float64=0.0,
+                        # solvable::Int=1,
+                        # timestamp::Union{DateTime,ZonedDateTime}=now(localzone()),
+                        # tags::Vector{Symbol}=Symbol[],
+                        # graphinit::Bool=getSolverParams(dfg).graphinit,
+                        # threadmodel=SingleThreaded,
+                        # inflation::Real=getSolverParams(dfg).inflation,
   #
   # depcrecation
 
@@ -1208,7 +1209,7 @@ function DFG.addFactor!(dfg::AbstractDFG,
 
   variables = getVariable.(dfg, xisyms)
   # verts = map(vid -> DFG.getVariable(dfg, vid), xisyms)
-  addFactor!(dfg, variables, usrfnc, multihypo=multihypo, nullhypo=nullhypo, solvable=solvable, tags=tags, graphinit=graphinit, threadmodel=threadmodel, timestamp=timestamp, inflation=inflation )
+  addFactor!(dfg, variables, usrfnc; suppressChecks=suppressChecks, kw... ) # multihypo=multihypo, nullhypo=nullhypo, solvable=solvable, tags=tags, graphinit=graphinit, threadmodel=threadmodel, timestamp=timestamp, inflation=inflation )
 end
 
 

diff --git a/src/GraphProductOperations.jl b/src/GraphProductOperations.jl
@@ -72,7 +72,7 @@ function localProduct(dfg::AbstractDFG,
 
   # make manifold belief from product
   vari = getVariable(dfg, sym)
-  pp = AMP.manikde!(pGM, getVariableType(vari) |> getManifold )
+  pp = AMP.manikde!(getManifold(getVariableType(vari)), pGM )
 
   return pp, dens, lb, sinfd
 end

diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl
@@ -422,6 +422,7 @@ include("Factors/GenericFunctions.jl")
 # Refactoring in progress
 include("Factors/MsgLikelihoods.jl")
 
+include("entities/GraphConstraintTypes.jl")
 include("CliqueTypes.jl")
 
 include("JunctionTreeTypes.jl")
@@ -444,7 +445,6 @@ include("TreeBasedInitialization.jl")
 include("HeatmapSampler.jl")
 
 # special variables and factors, see RoME.jl for more examples
-include("GraphConstraintTypes.jl")
 include("Factors/Mixture.jl")
 include("Factors/DefaultPrior.jl")
 include("Factors/LinearRelative.jl")

diff --git a/src/GraphConstraintTypes.jl → src/entities/GraphConstraintTypes.jl b/src/GraphConstraintTypes.jl → src/entities/GraphConstraintTypes.jl