tests should pass

Author: dehann

src/FactorGraph.jl

@@ -180,62 +180,37 @@ function setValKDE!(v::DFGVariable,
   nothing
 end
 function setValKDE!(v::DFGVariable,
-                    p::ManifoldKernelDensity,
+                    mkd::ManifoldKernelDensity,
                     setinit::Bool=true,
                     inferdim::Real=0;
                     solveKey::Symbol=:default  )
   #
   # @error("TESTING setValKDE! ", solveKey, string(listSolveKeys(v)))
-  setValKDE!(getSolverData(v,solveKey),p,setinit, Float64(inferdim))
+  setValKDE!(getSolverData(v,solveKey),mkd,setinit, Float64(inferdim))
   nothing
 end
 function setValKDE!(dfg::AbstractDFG,
                     sym::Symbol,
-                    p::ManifoldKernelDensity,
+                    mkd::ManifoldKernelDensity,
                     setinit::Bool=true,
                     inferdim::Real=0;
                     solveKey::Symbol=:default  )
   #
-  setValKDE!(getVariable(dfg, sym), p, setinit, inferdim, solveKey=solveKey)
+  setValKDE!(getVariable(dfg, sym), mkd, setinit, inferdim, solveKey=solveKey)
   nothing
 end
 
 
-"""
-    $SIGNATURES
-
-Set variable initialized status.
-"""
-function setVariableInitialized!( varid::VariableNodeData,
-                                  status::Bool)
-  #
-  varid.initialized = status
-end
-
-setVariableInitialized!(vari::DFGVariable, status::Bool) = setVariableInitialized!(getSolverData(vari), status)
-
-
-"""
-    $SIGNATURES
-
-Set method for the inferred dimension value in a variable.
-"""
-setVariableInferDim!(varid::VariableNodeData, val::Real) = varid.inferdim = convert(Float64,val)
-setVariableInferDim!(vari::DFGVariable, val::Real) = setVariableInferDim!(getSolverData(vari), val)
-
-## ==============================================================================================
-## ==============================================================================================
-
 
 function setValKDE!(vnd::VariableNodeData,
-                    p::ManifoldKernelDensity{M,B,Nothing},
+                    mkd::ManifoldKernelDensity{M,B,Nothing},
                     setinit::Bool=true,
                     inferdim::Union{Float32, Float64, Int32, Int64}=0 ) where {M,B}
   #
   # L==Nothing means no partials
-  ptsArr = AMP.getPoints(p) # , false) # for not partial
+  ptsArr = AMP.getPoints(mkd) # , false) # for not partial
   # also set the bandwidth
-  bws = getBW(p)[:,1]
+  bws = getBW(mkd)[:,1]
   setValKDE!(vnd,ptsArr,bws,setinit,inferdim )
   nothing
 end
@@ -260,6 +235,33 @@ function setValKDE!(vnd::VariableNodeData,
   nothing
 end
 
+
+"""
+    $SIGNATURES
+
+Set variable initialized status.
+"""
+function setVariableInitialized!( varid::VariableNodeData,
+                                  status::Bool)
+  #
+  varid.initialized = status
+end
+
+setVariableInitialized!(vari::DFGVariable, status::Bool) = setVariableInitialized!(getSolverData(vari), status)
+
+
+"""
+    $SIGNATURES
+
+Set method for the inferred dimension value in a variable.
+"""
+setVariableInferDim!(varid::VariableNodeData, val::Real) = varid.inferdim = convert(Float64,val)
+setVariableInferDim!(vari::DFGVariable, val::Real) = setVariableInferDim!(getSolverData(vari), val)
+
+## ==============================================================================================
+## ==============================================================================================
+
+
 """
     $SIGNATURES
 
@@ -807,8 +809,8 @@ function doautoinit!( dfg::AbstractDFG,
         end
         # FIXME ensure a product of only partial densities and returned pts are put to proper dimensions
         bel,inferdim = propagateBelief(dfg, getVariable(dfg,vsym), getFactor.(dfg,useinitfct), solveKey=solveKey, logger=logger)
-        @info "MANIFOLD IS" bel.manifold string(getPoints(bel, false)[1]) 
-        setValKDE!(xi, getPoints(bel, false), true, inferdim, solveKey=solveKey)
+        # @info "MANIFOLD IS" bel.manifold string(getPoints(bel, false)[1]) 
+        setValKDE!(xi, bel, true, inferdim, solveKey=solveKey) # getPoints(bel, false)
         # Update the estimates (longer DFG function used so cloud is also updated)
         setVariablePosteriorEstimates!(dfg, xi.label, solveKey)
         # Update the data in the event that it's not local

src/GraphProductOperations.jl

@@ -23,11 +23,13 @@ function propagateBelief( dfg::AbstractDFG,
                           needFreshMeasurements::Bool=true,
                           dbg::Bool=false,
                           logger=ConsoleLogger()  )
-    #
-
+  #
+  
   # get proposal beliefs
   destlbl = getLabel(destvar)
   inferdim = proposalbeliefs!(dfg, destlbl, factors, dens, solveKey=solveKey, N=N, dbg=dbg)
+  
+  # @show dens[1].manifold
 
   # make sure oldpts has right number of points
   oldBel = getBelief(dfg, destlbl, solveKey)
@@ -40,10 +42,13 @@ function propagateBelief( dfg::AbstractDFG,
   # few more data requirements
   varType = getVariableType(destvar)
   M = getManifold(varType)
-
+  # @info "BUILDING MKD" varType M
+  
   # take the product
   mkd = AMP.manifoldProduct(dens, M, Niter=1, oldPoints=oldpts)
 
+  # @info "GOT" mkd.manifold
+  
   return mkd, sum(inferdim)
 end
 

src/services/ApproxConv.jl

@@ -302,6 +302,7 @@ function proposalbeliefs!(dfg::AbstractDFG,
     inferd = getFactorSolvableDim(dfg, fct, destlbl, solveKey)
     # convolve or passthrough to get a new proposal
     propBel_ = calcProposalBelief(dfg, fct, destlbl, measurement, N=N, dbg=dbg, solveKey=solveKey)
+    # @show propBel_.manifold
     # partial density
     propBel = if isPartial(ccwl)
       pardims = _getDimensionsPartial(ccwl)