setMani with partial and more tests

Author: dehann

src/FGOSUtils.jl

@@ -152,7 +152,10 @@ function manikde!(pts::AbstractVector{P},
                   vartype::Union{InstanceType{<:InferenceVariable}, InstanceType{<:AbstractFactor}}) where P
   #
   M = getManifold(vartype)
-  infoPerCoord=ones(AMP.getNumberCoords(M, pts[1]))
+  # @info "WHAT" vartype, M
+  # @show pts[1]
+  # @show manifold_dimension(M)
+  infoPerCoord=ones(manifold_dimension(M))
   return AMP.manikde!(M, pts, infoPerCoord=infoPerCoord)
 end
 

src/services/EvalFactor.jl

@@ -359,26 +359,30 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
   spreadDist = spreadNH*sqrt(calcCovarianceBasic(mani, addEntr))
   # partials are treated differently
   ipc = if !isPartial(ccwl) #ccwl.partial
-      # TODO for now require measurements to be coordinates too
-      # @show typeof(ccwl.measurement[1])
-      for m in (1:length(addEntr))[ahmask]
-        # FIXME, selection for all measurement::Tuple elements
-        # @info "check broadcast" ccwl.usrfnc! addEntr[m] ccwl.measurement[1][m]
-        _setPointsMani!(addEntr[m], ccwl.measurement[1][m])
-      end
-      # ongoing part of RoME.jl #244
-      addEntropyOnManifold!(mani, addEntrNH, 1:getDimension(mani), spreadDist)
-      # do info per coords
-      ones(getDimension(Xi[sfidx]))
+    # TODO for now require measurements to be coordinates too
+    # @show typeof(ccwl.measurement[1])
+    for m in (1:length(addEntr))[ahmask]
+      # FIXME, selection for all measurement::Tuple elements
+      # @info "check broadcast" ccwl.usrfnc! addEntr[m] ccwl.measurement[1][m]
+      _setPointsMani!(addEntr[m], ccwl.measurement[1][m])
+    end
+    # ongoing part of RoME.jl #244
+    addEntropyOnManifold!(mani, addEntrNH, 1:getDimension(mani), spreadDist)
+    # do info per coords
+    ones(getDimension(Xi[sfidx]))
   else
     # FIXME but how to add partial factor info only on affected dimensions fro general manifold points?
     pvec = [fnc.partial...]
     # active hypo that receives the regular measurement information
     for m in (1:length(addEntr))[ahmask]
       # addEntr is no longer in coordinates, these are now general manifold points!!
-      for (i,dimnum) in enumerate(fnc.partial)
-        addEntr[m][dimnum] = ccwl.measurement[1][m][i]
-      end
+      # for (i,dimnum) in enumerate(fnc.partial)
+        # FIXME, need ability to replace partial points
+        partialCoords = ccwl.partialDims
+        Msrc, = getManifoldPartial(mani,partialCoords)
+        _setPointsManiPartial!(mani, addEntr[m], Msrc, ccwl.measurement[1][m], partialCoords)
+        # addEntr[m][dimnum] = ccwl.measurement[1][m][i]
+      # end
     end
     # null hypo mask that needs to be perturbed by "noise"
     addEntrNHp = view(addEntr, nhmask)

test/testSpecialEuclidean2Mani.jl

@@ -59,7 +59,7 @@ vnd = getVariableSolverData(fg, :x1)
 
 ##
 smtasks = Task[]
-solveTree!(fg; smtasks, verbose=true, recordcliqs=ls(fg))
+solveTree!(fg; smtasks, verbose=true) #, recordcliqs=ls(fg))
 # hists = fetchCliqHistoryAll!(smtasks);
 
 vnd = getVariableSolverData(fg, :x0)
@@ -92,11 +92,12 @@ p = addFactor!(fg, [:x0], mp, graphinit=false)
 
 ##
 
-# try
 pbel_ = approxConvBelief(fg, :x0f1, :x0)
-# catch 
-#   @test_broken false
-# end
+
+@test isPartial(pbel_)
+
+@test pbel_._partial == [1;2]
+@test length(pbel_.infoPerCoord) == 3
 
 ##
 end
@@ -141,7 +142,7 @@ vnd = getVariableSolverData(fg, :x6)
 ##
 
 smtasks = Task[]
-solveTree!(fg; smtasks)
+solveTree!(fg; smtasks);
 
 ##
 end