Merge pull request #779 from JuliaRobotics/21Q2/enh/printvar2

is/set Marginalized, printVar, bump v0.14.5

Project.toml

@@ -1,6 +1,6 @@
 name = "DistributedFactorGraphs"
 uuid = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
-version = "0.14.4"
+version = "0.14.5"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

src/DataBlobs/services/AbstractDataEntries.jl

@@ -29,7 +29,7 @@ buildSourceString(dfg::AbstractDFG, label::Symbol) =
 Get data entry
 """
 function getDataEntry(var::AbstractDFGVariable, key::Symbol)
-    !hasDataEntry(var, key) && error("Data entry $(key) does not exist in variable")
+    !hasDataEntry(var, key) && error("Data entry $(key) does not exist in variable $(getLabel(var))")
     return var.dataDict[key]
 end
 
@@ -42,7 +42,7 @@ end
 Add Data Entry to a DFG variable
 """
 function addDataEntry!(var::AbstractDFGVariable, bde::AbstractDataEntry)
-    haskey(var.dataDict, bde.label) && error("Data entry $(bde.label) already exists in variable")
+    haskey(var.dataDict, bde.label) && error("Data entry $(bde.label) already exists in variable $(getLabel(var))")
     var.dataDict[bde.label] = bde
     return bde
 end
@@ -66,7 +66,7 @@ DevNote
 - DF, unclear if `update` verb is applicable in this case, see #404
 """
 function updateDataEntry!(var::AbstractDFGVariable,  bde::AbstractDataEntry)
-    !haskey(var.dataDict, bde.label) && (@warn "$(bde.label) does not exist in variable, adding")
+    !haskey(var.dataDict, bde.label) && (@warn "$(bde.label) does not exist in variable $(getLabel(var)), adding")
     var.dataDict[bde.label] = bde
     return bde
 end

src/DistributedFactorGraphs.jl

@@ -180,7 +180,7 @@ export VariableNodeData, PackedVariableNodeData
 
 export packVariableNodeData, unpackVariableNodeData
 
-export getSolvedCount, isSolved, setSolvedCount!, isInitialized
+export getSolvedCount, isSolved, setSolvedCount!, isInitialized, isMarginalized, setMarginalized!
 
 export getNeighborhood, getNeighbors, _getDuplicatedEmptyDFG
 export findFactorsBetweenNaive

src/services/CustomPrinting.jl

@@ -16,46 +16,68 @@ function printVariable( io::IO, vert::DFGVariable;
     if short
         # opmemt = (getVariableType(vert) |> typeof ).name
         vari = getVariableType(vert) |> typeof
-        printstyled(ioc, typeof(vert).name, "{",vari.name,"...}","\n", bold=true)
+        printstyled(ioc, typeof(vert).name.name, "{",bold=true)
+        printstyled(ioc, vari.name.name, bold=true, color=:blue)
+        printstyled(ioc,"...}", bold=true)
+        println(ioc, "")
         # printstyled(ioc, summary(vert),"\n", bold=true)
 
-        vnd = getSolverData(vert)
-        println(ioc, "  timestamp:  ", vert.timestamp)
-        println(ioc, "  label:      ", vert.label)
-        println(ioc, "  solvable:   ", getSolvable(vert))
-        println(ioc, "  initilized: ", isInitialized(vert))
-        println(ioc, "  tags: ", getTags(vert))
         try
-            print(ioc, "  manifold: ")
+            print(ioc, "  manifold:   ")
             show(ioc, getManifold(vert))
             println(ioc, "")
         catch e
         end
+        vnd = getSolverData(vert, :default)
+        println(ioc, "  timestamp:  ", vert.timestamp)
+        println(ioc, "   nstime:    ", vert.nstime)
+        print(ioc,   "  label:      ")
+        printstyled(ioc, vert.label, bold=true)
+        println(ioc)
+        println(ioc, "  solvable:   ", getSolvable(vert))
+        println(ioc, "  tags:       ", getTags(vert))
         solk = listSolveKeys(vert) |> collect
         lsolk = length(solk)
         smsk = (rand(1:lsolk,100) |> unique)[1:minimum([4,lsolk])]
-        println(ioc, "  Nr solveKeys=$(lsolk): ", solk[smsk], 4<lsolk ? "..." : "")
-        printstyled(ioc, "    :default <-- VariableNodeData", "\n", bold=true)
-        println(ioc, "      size marginal samples: ", size(vnd.val))
-        println(ioc, "      kde bandwidths: ", round.((vnd.bw)[:,1], digits=4))
+        # list the marginalization status
+        ismarg = solk .|> x->isMarginalized(vert, x)
+        isinit = solk .|> x->isInitialized(vert, x)
+        printstyled(ioc, "  # VND solveKeys=    ($(lsolk))", bold=true)
+        println(ioc, "")
+        printstyled(ioc, "  # initialized:      ", bold=true)
+        println(ioc, "(true=", sum(isinit), ",false=", length(isinit) - sum(isinit), ")" )
+        printstyled(ioc, "  # marginalized:     ", bold=true)
+        println(ioc, "(true=", sum(ismarg), ",false=", length(ismarg) - sum(ismarg), ")" )
+
+        println(ioc, "    :default <-- VariableNodeData")
+        println(ioc, "      initilized:        ", isInitialized(vert, :default))
+        println(ioc, "      marginalized:      ", isMarginalized(vert, :default))
+        println(ioc, "      size bel. samples: ", size(vnd.val))
+        println(ioc, "      kde bandwidths:    ", round.((vnd.bw)[:,1], digits=4))
+        printstyled(ioc, "     VNDs: ",bold=true)
+        println(ioc, solk[smsk], 4<lsolk ? "..." : "")
+        printstyled(ioc, "  # PPE solveKeys=    ($(length(getPPEDict(vert))))", bold=true)
+        println(ioc, "")
         if haskey(getPPEDict(vert), :default)
-            printstyled(ioc, "    :default ", bold=true)
-            println(ioc, "<-- PPE.suggested: ", round.(ppe.suggested,digits=4) )
+            print(ioc, "    :default ")
+            println(ioc, "<-- .suggested:    ", round.(getPPE(vert, :default).suggested,digits=4) )
         end
-        maxkeys = 3
+        maxkeys = 4
         for (key, ppe) in getPPEDict(vert)
+            key == :default && continue # skip as default is done separately
             maxkeys -= 1
             maxkeys == 0 && break
-            printstyled(ioc, "    :$key ", bold=true)
-            println(ioc, "<-- PPE.suggested: ", round.(ppe.suggested,digits=4) )
+            print(ioc, "    :$key ")
+            println(ioc, "<-- .suggested:  ", round.(ppe.suggested,digits=4) )
         end
-        printstyled(ioc, "  TYPE: ", bold=true, color=:blue)
+        printstyled(ioc, "  VariableType: ", color=:blue, bold=true)
         println(ioc, vari)
         # println(ioc, "kde max: $(round.(getKDEMax(getKDE(vnd)),digits=4))")
         # println(ioc, "kde max: $(round.(getKDEMax(getKDE(vnd)),digits=4))")
     else
 
-        printstyled(ioc, summary(vert),"\n", bold=true, color=:blue)
+        printstyled(ioc, summary(vert), bold=true, color=:blue)
+        println(ioc, "")
 
         :solver in skipfields && push!(skipfields, :solverDataDict)
         :ppe in skipfields && push!(skipfields, :ppeDict)
@@ -65,7 +87,8 @@ function printVariable( io::IO, vert::DFGVariable;
         nf = nfields(vert)
 
         for f in fields
-            printstyled(ioc, f,":\n", color=:blue)
+            printstyled(ioc, f,":", color=:blue)
+            println(ioc, "")
             show(ioc, getproperty(vert, f))
             println(ioc)
         end
@@ -85,20 +108,22 @@ function printFactor(   io::IO, vert::DFGFactor;
         opmemt = (getSolverData(vert).fnc |> typeof).name
         fct = getFactorType(vert)
         fctt = fct |> typeof
-        printstyled(ioc, typeof(vert).name, "{",opmemt,"{",fctt.name,"...}}","\n", bold=true)
-        println(ioc, "  timestamp: ", vert.timestamp)
-        println(ioc, "  nstime: ",vert.nstime)
-        println(ioc, "  label: ", vert.label)
-        println(ioc, "  solvable: ", vert.solvable)
+        printstyled(ioc, typeof(vert).name.name, "{",opmemt,"{",fctt.name.name,"...}}","\n", bold=true)
+        println(ioc, "  timestamp:     ", vert.timestamp)
+        println(ioc, "   nstime:       ", vert.nstime)
+        print(ioc,   "  label:         ")
+        printstyled(ioc, vert.label, bold=true)
+        println(ioc)
+        println(ioc, "  solvable:      ", vert.solvable)
         println(ioc, "  VariableOrder: ", vert._variableOrderSymbols)
-        println(ioc, "  multihypo: ", getSolverData(vert).multihypo) # FIXME #477
-        println(ioc, "  nullhypo: ", getSolverData(vert).nullhypo)
-        println(ioc, "  tags: ", vert.tags)
-        printstyled(ioc, "  TYPE: ", bold=true, color=:blue)
+        println(ioc, "  multihypo:     ", getSolverData(vert).multihypo) # FIXME #477
+        println(ioc, "  nullhypo:      ", getSolverData(vert).nullhypo)
+        println(ioc, "  tags:          ", vert.tags)
+        printstyled(ioc, "  FactorType: ", bold=true, color=:blue)
         println(ioc, fctt)
         # show(ioc, fctt)
         for f in setdiff(fieldnames(fctt), skipfields)
-            printstyled(ioc, f,":\n", color=:blue)
+            printstyled(ioc, f,":\n", color=:magenta)
             show(ioc, getproperty(fct, f))
             println(ioc)
         end

src/services/DFGVariable.jl

@@ -190,6 +190,29 @@ function isInitialized(dfg::AbstractDFG, label::Symbol, key::Symbol=:default)
 end
 
 
+"""
+    $SIGNATURES
+
+Return `::Bool` on whether this variable has been marginalized.
+
+Notes:
+- VariableNodeData default `solveKey=:default`
+"""
+isMarginalized(vert::DFGVariable, solveKey::Symbol=:default) = getSolverData(vert, solveKey).ismargin
+isMarginalized(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol=:default) = isMarginalized(DFG.getVariable(dfg, sym), solveKey)
+
+"""
+    $SIGNATURES
+
+Mark a variable as marginalized `true` or `false`.
+"""
+function setMarginalized!(vnd::VariableNodeData, val::Bool)
+  vnd.ismargin = val
+end
+setMarginalized!(vari::DFGVariable, val::Bool, solveKey::Symbol=:default) = setMarginalized!(getSolverData(vari, solveKey), val)
+setMarginalized!(dfg::AbstractDFG, sym::Symbol, val::Bool, solveKey::Symbol=:default) = setMarginalized!(getVariable(dfg, sym), val, solveKey)
+
+
 ##==============================================================================
 ## Variables
 ##==============================================================================