Merge pull request #1288 from JuliaRobotics/21Q3/enh/vecpproper

wip, Vector{P} proper

Project.toml

@@ -43,7 +43,7 @@ TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 
 [compat]
-ApproxManifoldProducts = "0.4"
+ApproxManifoldProducts = "0.4.1"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"

src/ApproxConv.jl

@@ -841,7 +841,7 @@ function findRelatedFromPotential(dfg::AbstractDFG,
   Npoints = length(pts) # size(pts,2)
   # Assume we only have large particle population sizes, thanks to addNode!
   M = getManifold(getVariableType(dfg, target))
-  proposal = AMP.manikde!(pts, M)
+  proposal = AMP.manikde!(M, pts)
 
   # FIXME consolidate with approxConv method instead
   if Npoints != N # this is where we control the overall particle set size

src/BeliefTypes.jl

@@ -60,7 +60,7 @@ TreeBelief( val::AbstractVector{P},
             inferdim::Real=0,
             variableType::T=ContinuousScalar(),
             manifold::M=getManifold(variableType),
-            solvableDim::Real=0) where {P <: AbstractVector, T <: InferenceVariable, M <:MB.AbstractManifold} = TreeBelief{T,P,M}(val, bw, inferdim, variableType, manifold, solvableDim)
+            solvableDim::Real=0) where {P, T <: InferenceVariable, M <:MB.AbstractManifold} = TreeBelief{T,P,M}(val, bw, inferdim, variableType, manifold, solvableDim)
 
 function TreeBelief(vnd::VariableNodeData{T}, solvDim::Real=0) where T
   TreeBelief( vnd.val, vnd.bw, vnd.inferdim, getVariableType(vnd), getManifold(T), solvDim )

src/CompareUtils.jl

@@ -21,10 +21,13 @@ function Base.isapprox( p1::Union{<:BallTreeDensity, <:ManifoldKernelDensity},
   mmd(p1,p2) < atol
 end
 
-function compareAllSpecial(A::T1, B::T2;
-                    skip=Symbol[], show::Bool=true) where {T1 <: CommonConvWrapper, T2 <: CommonConvWrapper}
+function compareAllSpecial( A::T1, B::T2;
+                            skip=Symbol[], show::Bool=true) where {T1 <: CommonConvWrapper, T2 <: CommonConvWrapper}
   #
-  T1 != T2 && return false
+  if T1 != T2
+    @warn "CCW types not equal" T1 T2
+    return false
+  end
   return compareAll(A, B, skip=skip, show=show)
 end
 

src/DeconvUtils.jl

@@ -59,7 +59,8 @@ function approxDeconv(fcto::DFGFactor,
   fmd = _getFMdThread(ccw)
 
   # TODO assuming vector on only first container in measurement::Tuple
-  makeTarget = (i) -> view(measurement[1][i],:)
+  makeTarget = (i) -> measurement[1][i] # TODO does not support copy-primitive types like Float64, only Ref()
+  # makeTarget = (i) -> view(measurement[1][i],:)
   # makeTarget = (i) -> view(measurement[1], :, i)
 
   # NOTE 

src/Deprecated.jl

@@ -25,6 +25,11 @@ end
 ## Deprecate code below before v0.27
 ##==============================================================================
 
+# getManifold(::InstanceType{LinearRelative{N}}) where {N} = Euclidean(N)
+# getManifolds(::T) where {T <: LinearRelative} = convert(Tuple, getManifold(T))
+# getManifolds(::Type{<:T}) where {T <: LinearRelative} = convert(Tuple, getManifold(T))
+# getManifolds(fctType::Type{LinearRelative}) = getManifolds(getDomain(fctType))
+
 # # FIXME, why is Manifolds depdendent on the solveKey?? Should just be at DFGVariable level?
 
 # getManifolds(vd::VariableNodeData) = getVariableType(vd) |> getManifolds

src/FGOSUtils.jl

@@ -130,14 +130,14 @@ function manikde!(pts::AbstractVector{P},
                   variableType::Union{InstanceType{<:InferenceVariable}, InstanceType{<:AbstractFactor}}  ) where P
   #
   M = getManifold(variableType)
-  return AMP.manikde!(pts, bws, M)
+  return AMP.manikde!(M, pts, bws)
 end
 
 function manikde!(pts::AbstractVector{P}, 
                   vartype::Union{InstanceType{<:InferenceVariable}, InstanceType{<:AbstractFactor}}) where P
   #
   M = getManifold(vartype)
-  return AMP.manikde!(pts, M)
+  return AMP.manikde!(M, pts)
 end
 
 # manikde!( pts::AbstractVector{<:Real}, 

src/FactorGraph.jl

@@ -85,33 +85,33 @@ function setBW!(v::DFGVariable, bw::Array{Float64,2}; solveKey::Symbol=:default)
   nothing
 end
 
-function setVal!(vd::VariableNodeData, val::AbstractVector{P}) where P <:AbstractVector
+function setVal!(vd::VariableNodeData, val::AbstractVector{P}) where P
   vd.val = val
   nothing
 end
-function setVal!(v::DFGVariable, val::AbstractVector{P}; solveKey::Symbol=:default) where P <:AbstractVector
+function setVal!(v::DFGVariable, val::AbstractVector{P}; solveKey::Symbol=:default) where P
     setVal!(getSolverData(v, solveKey), val)
     nothing
 end
-function setVal!(vd::VariableNodeData, val::AbstractVector{P}, bw::Array{Float64,2}) where P <:AbstractVector
+function setVal!(vd::VariableNodeData, val::AbstractVector{P}, bw::Array{Float64,2}) where P
     setVal!(vd, val)
     setBW!(vd, bw)
     nothing
 end
-function setVal!(v::DFGVariable, val::AbstractVector{P}, bw::Array{Float64,2}; solveKey::Symbol=:default) where P <:AbstractVector
+function setVal!(v::DFGVariable, val::AbstractVector{P}, bw::Array{Float64,2}; solveKey::Symbol=:default) where P
   setVal!(v, val, solveKey=solveKey)
   setBW!(v, bw, solveKey=solveKey)
   nothing
 end
-function setVal!(vd::VariableNodeData, val::AbstractVector{P}, bw::Vector{Float64}; solveKey::Symbol=:default) where P <:AbstractVector
+function setVal!(vd::VariableNodeData, val::AbstractVector{P}, bw::Vector{Float64}) where P
   setVal!(vd, val, reshape(bw,length(bw),1))
   nothing
 end
-function setVal!(v::DFGVariable, val::AbstractVector{P}, bw::Vector{Float64}; solveKey::Symbol=:default) where P <:AbstractVector
+function setVal!(v::DFGVariable, val::AbstractVector{P}, bw::Vector{Float64}; solveKey::Symbol=:default) where P
   setVal!(getSolverData(v, solveKey=solveKey), val, bw)
   nothing
 end
-function setVal!(dfg::AbstractDFG, sym::Symbol, val::AbstractVector{P}; solveKey::Symbol=:default) where P <:AbstractVector
+function setVal!(dfg::AbstractDFG, sym::Symbol, val::AbstractVector{P}; solveKey::Symbol=:default) where P
   setVal!(getVariable(dfg, sym), val, solveKey=solveKey)
 end
 
@@ -128,7 +128,7 @@ function setValKDE!(vd::VariableNodeData,
                     pts::AbstractVector{P},
                     bws::Vector{Float64},
                     setinit::Bool=true,
-                    inferdim::Float64=0.0 ) where P <:AbstractVector
+                    inferdim::Float64=0.0 ) where P
   #
   setVal!(vd, pts, bws) # BUG ...al!(., val, . ) ## TODO -- this can be a little faster
   setinit ? (vd.initialized = true) : nothing
@@ -139,7 +139,7 @@ end
 function setValKDE!(vd::VariableNodeData,
                     val::AbstractVector{P},
                     setinit::Bool=true,
-                    inferdim::Real=0.0  ) where P <:AbstractVector
+                    inferdim::Real=0.0  ) where P
   # recover variableType information
   varType = getVariableType(vd)
   p = AMP.manikde!(val, varType)
@@ -152,7 +152,7 @@ function setValKDE!(v::DFGVariable,
                     bws::Array{<:Real,2},
                     setinit::Bool=true,
                     inferdim::Float64=0;
-                    solveKey::Symbol=:default) where P <:AbstractVector
+                    solveKey::Symbol=:default) where P
   # recover variableType information
   setValKDE!(getSolverData(v, solveKey), val, bws[:,1], setinit, inferdim )
 
@@ -163,7 +163,7 @@ function setValKDE!(v::DFGVariable,
                     val::AbstractVector{P},
                     setinit::Bool=true,
                     inferdim::Float64=0.0;
-                    solveKey::Symbol=:default) where P <:AbstractVector
+                    solveKey::Symbol=:default) where P
   # recover variableType information
   setValKDE!(getSolverData(v, solveKey),val, setinit, inferdim )
   nothing
@@ -486,6 +486,18 @@ function addVariable!(dfg::AbstractDFG,
   return v
 end
 
+function _resizePointsVector!(vecP::AbstractVector{P}, mkd::ManifoldKernelDensity, N::Int) where P
+  #
+  pN = length(vecP)
+  resize!(vecP, N)
+  for j in pN:N
+    smp = AMP.sample(mkd, 1)[1]
+    # @show j, smp, typeof(smp), typeof(vecP[j])
+    vecP[j] = smp[1]
+  end
+
+  vecP
+end
 
 
 """
@@ -503,47 +515,34 @@ Notes
 - for initialization, solveFor = Nothing.
 - `P = getPointType(<:InferenceVariable)`
 """
-function prepareparamsarray!( ARR::AbstractVector{P},
+function prepareparamsarray!( ARR::AbstractVector{<:AbstractVector{P}},
                               Xi::Vector{<:DFGVariable},
                               solvefor::Union{Nothing, Symbol},
                               N::Int=0;
-                              solveKey::Symbol=:default  ) where P <: AbstractVector
+                              solveKey::Symbol=:default  ) where P
   #
   LEN = Int[]
   maxlen = N # FIXME see #105
   count = 0
   sfidx = 0
 
   for xi in Xi
-    push!(ARR, getVal(xi, solveKey=solveKey))
+    vecP = getVal(xi, solveKey=solveKey)
+    push!(ARR, vecP)
     LEN = length.(ARR)
     maxlen = maximum(LEN)
-    # @show len = size(ARR[end], 2)
-    # push!(LEN, len)
-    # if len > maxlen
-    #   maxlen = len
-    # end
     count += 1
     if xi.label == solvefor
       sfidx = count #xi.index
     end
   end
 
-  # resample variables with too few kernels
+  # resample variables with too few kernels (manifolds points)
   SAMP = LEN .< maxlen
   for i in 1:count
     if SAMP[i]
       Pr = getBelief(Xi[i], solveKey)
-      resize!(ARR[i], maxlen)
-      for j in 1:maxlen
-        smp = AMP.sample(Pr, 1)[1]
-        arr_i = ARR[i]
-        if isdefined(arr_i, j)
-          arr_i[j][:] = smp[:]
-        else
-          arr_i[j] = smp[:]
-        end
-      end
+      _resizePointsVector!(ARR[i], Pr, maxlen)
     end
   end
 
@@ -617,17 +616,16 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
                                 inflation::Real=0.0  ) where {T <: FunctorInferenceType}
   #
   pttypes = getVariableType.(Xi) .|> getPointType
-  sametype = 0 < length(pttypes) ? all( pttypes[1] .== pttypes ) : true
-  P_type = 0 < length(pttypes) ? Vector{pttypes[1]} : Vector{Float64}
-  @assert sametype "Current implementation only allows for same point type: $pttypes"
-  ARR = Vector{P_type}()
+  PointType = 0 < length(pttypes) ? pttypes[1] : Vector{Float64}
+  ARR = Vector{Vector{PointType}}()
   maxlen, sfidx, mani = prepareparamsarray!(ARR, Xi, nothing, 0) # Nothing for init.
-  # fldnms = fieldnames(T) # typeof(usrfnc)
 
   # standard factor metadata
   sflbl = 0==length(Xi) ? :null : getLabel(Xi[end])
   fmd = FactorMetadata(Xi, getLabel.(Xi), ARR, sflbl, nothing)
-  cf = CalcFactor( usrfnc, fmd, 0, 1, (Vector{Vector{Float64}}(),), ARR)
+  # guess measurement points type
+  MeasType = Vector{Float64} # FIXME use `usrfnc` to get this information instead
+  cf = CalcFactor( usrfnc, fmd, 0, 1, (Vector{MeasType}(),), ARR)
 
   zdim = calcZDim(cf)
   # zdim = T != GenericMarginal ? size(getSample(usrfnc, 2)[1],1) : 0
@@ -643,7 +641,7 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
 
   ccw = CommonConvWrapper(
           usrfnc,
-          P_type(),
+          PointType[],
           zdim,
           ARR,
           fmd,
@@ -984,7 +982,7 @@ end
 function initManual!( dfg::AbstractDFG, 
                       sym::Symbol, 
                       pts::AbstractVector{P}, 
-                      solveKey::Symbol=:default ) where {P <: AbstractVector}
+                      solveKey::Symbol=:default ) where {P}
   #
   var = getVariable(dfg, sym)
   pp = manikde!(pts, getManifold(var))

src/Factors/LinearRelative.jl

@@ -28,12 +28,7 @@ LinearRelative(nm::Distributions.ContinuousUnivariateDistribution) = LinearRelat
 LinearRelative(nm::MvNormal) = LinearRelative{length(nm.μ), typeof(nm)}(nm)
 LinearRelative(nm::Union{<:BallTreeDensity,<:ManifoldKernelDensity}) = LinearRelative{Ndim(nm), typeof(nm)}(nm)
 
-# getManifold(::InstanceType{LinearRelative{N}}) where {N} = Euclidean(N)
-# getManifolds(::T) where {T <: LinearRelative} = convert(Tuple, getManifold(T))
-# getManifolds(::Type{<:T}) where {T <: LinearRelative} = convert(Tuple, getManifold(T))
-# getManifolds(fctType::Type{LinearRelative}) = getManifolds(getDomain(fctType))
-
-getManifold(::InstanceType{LinearRelative{N}}) where N = ContinuousEuclid{N}
+getManifold(::InstanceType{LinearRelative{N}}) where N = getManifold(ContinuousEuclid{N})
 
 # TODO standardize
 getDimension(::InstanceType{LinearRelative{N}}) where {N} = N

src/ParametricCSMFunctions.jl

@@ -142,7 +142,7 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer)
         logCSM(csmc, "$(csmc.cliq.id) down: updating $v : $val"; loglevel=Logging.Info)
         vnd = getSolverData(getVariable(csmc.cliqSubFg, v), :parametric)
         #Update subfg variables
-        vnd.val .= val.val
+        vnd.val[1] .= val.val
         vnd.bw .= val.cov
       end
     else

src/ParametricUtils.jl

@@ -340,7 +340,7 @@ function solveConditionalsParametric(fg::AbstractDFG,
   flatvar = FlatVariables(fg, varIds)
 
   for vId in varIds
-    flatvar[vId] = getVariableSolverData(fg, vId, solvekey).val[:,1]
+    flatvar[vId] = getVariableSolverData(fg, vId, solvekey).val[1][:]
   end
   initValues = flatvar.X
 

src/TreeMessageUtils.jl

@@ -105,7 +105,7 @@ end
 
 
 function generateMsgPrior(belief_::TreeBelief, ::NonparametricMessage)
-  kdePr = manikde!(belief_.val, belief_.bw[:,1], getManifold(belief_.variableType))
+  kdePr = manikde!(getManifold(belief_.variableType), belief_.val, belief_.bw[:,1])
   MsgPrior(kdePr, belief_.inferdim)
 end