wip on new setPartial

Author: dehann

src/Deprecated.jl

@@ -133,6 +133,29 @@ end
 ## Deprecate code below before v0.26
 ##==============================================================================
 
+# wow, that was quite far off -- needs testing
+# function factorCanInitFromOtherVars(dfg::T,
+#                                     fct::Symbol,
+#                                     loovar::Symbol)::Tuple{Bool, Vector{Symbol}, Vector{Symbol}} where T <: AbstractDFG
+#   #
+#   # all variables attached to this factor
+#   varsyms = getNeighbors(dfg, fct)
+#
+#   # list of factors to use in init operation
+#   useinitfct = Symbol[]
+#   faillist = Symbol[]
+#   for vsym in varsyms
+#     xi = DFG.getVariable(dfg, vsym)
+#     if (isInitialized(xi) && sum(useinitfct .== fct) == 0 ) || length(varsyms) == 1
+#       push!(useinitfct, fct)
+#     end
+#   end
+#
+#   return (length(useinitfct)==length(varsyms)&&length(faillist)==0,
+#           useinitfct,
+#           faillist   )
+# end
+
 @deprecate calcPerturbationFromVariable( fgc::FactorGradientsCached!, fromVar::Int, infoPerCoord::AbstractVector;tol::Real=0.02*fgc._h ) calcPerturbationFromVariable(fgc, [fromVar => infoPerCoord;], tol=tol )
 
 function getVal(vA::Vector{<:DFGVariable}, solveKey::Symbol=:default)

src/FactorGraph.jl

@@ -738,9 +738,9 @@ function factorCanInitFromOtherVars(dfg::AbstractDFG,
     mhp = getMultihypoDistribution(fctnode).p
     allmhp,certainidx,uncertnidx = getHypothesesVectors(mhp)
     if isLeastOneHypoAvailable(sfidx, certainidx, uncertnidx, isinit)
-       # special case works
-       @info "allowing init from incomplete set of previously initialized hypotheses, fct=$fct"
-       canuse = true
+      # special case works
+      @info "allowing init from incomplete set of previously initialized hypotheses, fct=$fct"
+      canuse = true
     end
   end
 
@@ -754,29 +754,6 @@ function factorCanInitFromOtherVars(dfg::AbstractDFG,
 end
 
 
-# wow, that was quite far off -- needs testing
-# function factorCanInitFromOtherVars(dfg::T,
-#                                     fct::Symbol,
-#                                     loovar::Symbol)::Tuple{Bool, Vector{Symbol}, Vector{Symbol}} where T <: AbstractDFG
-#   #
-#   # all variables attached to this factor
-#   varsyms = getNeighbors(dfg, fct)
-#
-#   # list of factors to use in init operation
-#   useinitfct = Symbol[]
-#   faillist = Symbol[]
-#   for vsym in varsyms
-#     xi = DFG.getVariable(dfg, vsym)
-#     if (isInitialized(xi) && sum(useinitfct .== fct) == 0 ) || length(varsyms) == 1
-#       push!(useinitfct, fct)
-#     end
-#   end
-#
-#   return (length(useinitfct)==length(varsyms)&&length(faillist)==0,
-#           useinitfct,
-#           faillist   )
-# end
-
 """
     $(SIGNATURES)
 
@@ -830,6 +807,7 @@ 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)
         # Update the estimates (longer DFG function used so cloud is also updated)
         setVariablePosteriorEstimates!(dfg, xi.label, solveKey)

src/SolverUtilities.jl

@@ -21,6 +21,7 @@ Related
 randToPoints(distr::SamplableBelief, N::Int=1) = [rand(distr,1)[:] for i in 1:N]
 randToPoints(distr::ManifoldKernelDensity, N::Int=1) = rand(distr,N)
 
+
 _setPointsMani!(dest::AbstractVector, src::AbstractVector) = (dest .= src)
 _setPointsMani!(dest::AbstractMatrix, src::AbstractMatrix) = (dest .= src)
 function _setPointsMani!(dest::AbstractVector, src::AbstractMatrix)
@@ -43,6 +44,33 @@ function _setPointsMani!(dest::ProductRepr, src::ProductRepr)
   end
 end
 
+# asPartial=true indicates that src coords are smaller than dest coords, and false implying src has dummy values in placeholder dimensions
+function _setPointsManiPartial!(Mdest::AbstractManifold, 
+                                dest, 
+                                Msrc::AbstractManifold, 
+                                src, 
+                                partial::AbstractVector{<:Integer},
+                                asPartial::Bool=true )
+  #
+
+  e0 = identity(Mdest, dest)
+  dest_ = vee(Mdest, e0, log(Mdest, e0, dest))
+
+  e0s = identity(Msrc, src)
+  src_ = vee(Msrc, e0s, log(Msrc, e0s, src))
+
+  # do the copy in coords 
+  dest_[partial] .= asPartial ? src_ : view(src_, partial)
+
+  # update points base in original
+  dest__ = exp(Mdest, e0, hat(Mdest, e0, dest_))
+  _setPointsMani!(dest, dest__)
+
+  #
+  return dest 
+end
+
+
 
 """
     $TYPEDSIGNATURES

src/entities/OptionalDensities.jl

@@ -82,6 +82,6 @@ function Base.show(io::IO, mime::MIME"text/plain", x::HeatmapDensityRegular{T,H,
   nothing
 end
 
-# Base.show(io::IO, ::MIME"application/prs.juno.inline", x::HeatmapDensityRegular) = x
+Base.show(io::IO, ::MIME"application/prs.juno.inline", x::HeatmapDensityRegular) = show(io,x)
 
 #

test/testApproxConv.jl

@@ -78,9 +78,23 @@ pts_ = approxConv(fg, :x1f1, :l2)
 @test 1.6 < Statistics.std(pts) < 4.0
 
 ##
-
 end
 
 
+@testset "test approxConvBelief with partial prior" begin
+##
+
+fg = initfg()
+
+addVariable!(fg, :x0, ContinuousEuclid{2})
+pp = PartialPrior(Normal(),(2,))
+addFactor!(fg, [:x0], pp, graphinit=false)
+
+approxConvBelief(fg, :x0f1, :x0)
+
+@info "second test with more complicated manifolds in testSpecialEuclidean2Mani.jl"
+
+##
+end
 
 #

test/testSpecialEuclidean2Mani.jl

@@ -36,6 +36,9 @@ v0 = addVariable!(fg, :x0, SpecialEuclidean2)
 mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
 p = addFactor!(fg, [:x0], mp)
 
+
+##
+
 doautoinit!(fg, :x0)
 
 ##
@@ -78,6 +81,23 @@ f = addFactor!(fg, [:x1, :x2], mf)
 @test solveTree!(fg; verbose=true) isa Tuple
 
 
+## test partial prior issue
+
+fg = initfg()
+
+v0 = addVariable!(fg, :x0, SpecialEuclidean2)
+mp = PartialPrior(MvNormal([0.01, 0.01]), (1,2))
+
+p = addFactor!(fg, [:x0], mp, graphinit=false)
+
+##
+
+# try
+pbel_ = approxConvBelief(fg, :x0f1, :x0)
+# catch 
+#   @test_broken false
+# end
+
 ##
 end
 
@@ -190,4 +210,7 @@ vnd = getVariableSolverData(fg, :x0)
 vnd = getVariableSolverData(fg, :x1)
 @test all(isapprox.(mean(vnd.val), [1.0,2.0], atol=0.1))
 
-end
+end
+
+
+#