better IPC on ARel partial, towards #1394,

xref #1010
JuliaRobotics · Sep 29, 2021 · 5abf1ca · 5abf1ca
1 parent eaed034
commit 5abf1ca
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 2 deletions.
diff --git a/src/GraphProductOperations.jl b/src/GraphProductOperations.jl
@@ -42,7 +42,7 @@ function propagateBelief( dfg::AbstractDFG,
   # few more data requirements
   varType = getVariableType(destvar)
   M = getManifold(varType)
-  # @info "BUILDING MKD" varType M
+  # @info "BUILDING MKD" varType M isPartial.(dens)
 
   # take the product
   mkd = AMP.manifoldProduct(dens, M, Niter=1, oldPoints=oldpts, N=N)

diff --git a/src/services/ApproxConv.jl b/src/services/ApproxConv.jl
@@ -165,6 +165,7 @@ function calcProposalBelief(dfg::AbstractDFG,
 end
 
 
+# specifically the PartialPriorPassThrough dispatch
 function calcProposalBelief(dfg::AbstractDFG,
                             fct::DFGFactor{<:CommonConvWrapper{<:PartialPriorPassThrough}},
                             target::Symbol,

diff --git a/src/services/EvalFactor.jl b/src/services/EvalFactor.jl
@@ -341,9 +341,16 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
                             inflateCycles=inflateCycles, skipSolve=skipSolve,
                             _slack=_slack )
   #
-  # ## do info per coord
+  # FIXME do info per coord
   # ipc_ = _calcIPCRelative(Xi, ccwl, hyporecipe, sfidx)
   ipc = ones(getDimension(Xi[sfidx]))
+  if isPartial(ccwl)
+    # FIXME this is a workaround until better _calcIPCRelative can be used
+    msk_ = setdiff(1:length(ipc), ccwl.usrfnc!.partial)
+    for _i in msk_
+      ipc[_i] = 0.0
+    end
+  end
 
   # return the found points, and info per coord
   return ccwl.params[ccwl.varidx], ipc