Merge pull request #64 from JuliaRobotics/hotfix/4Q19/plotfactor

add reportFactors

src/PlotFactors.jl

@@ -1,14 +1,55 @@
 
+
+function plotFactorMeasurements(dfg::AbstractDFG, fctsym::Symbol, fct::FunctorInferenceType; hdl=[])
+  @error "plotFactorMeasurements not implemented yet for $(typeof(fct))."
+end
+
+function plotFactorMeasurements(dfg::AbstractDFG, fctsym::Symbol, fct::Pose2Pose2; hdl=[])
+  #
+  me, me0 = solveFactorMeasurements(dfg, fctsym)
+
+  pt = plotKDE([manikde!(me[1:2,:], Point2);manikde!(me0[1:2,:], Point2)], c=["red";"blue"], legend=["pred";"meas"], levels=3)
+  pc = plotKDECircular([manikde!(me[3:3,:], Sphere1);manikde!(me0[3:3,:], Sphere1)], c=["red";"blue"], legend=["pred";"meas"], title="inv. solve, Pose2Pose2, $fctsym")
+
+  push!(hdl, pt)
+  push!(hdl, pc)
+
+  hstack(pt, pc)
+end
+
+
+function plotFactorMeasurements(dfg::AbstractDFG,
+                                fctsym::Symbol,
+                                fct::Pose2Point2BearingRange;
+                                hdl=[] )
+  #
+  me, me0 = solveFactorMeasurements(dfg, fctsym)
+
+  pc = plotKDECircular([manikde!(me[1:1,:], Sphere1);manikde!(me0[1:1,:], Sphere1)], c=["red";"blue"], legend=["pred";"meas"], title="inv. solve, Pose2Point2BearingRange, $fctsym")
+  pcl = plotKDE([manikde!(me[1:1,:], ContinuousScalar);manikde!(me0[1:1,:], ContinuousScalar)], c=["red";"blue"], legend=["pred";"meas"], title="unwrapped rotation, should be [-pi,pi)")
+  pl = plotKDE([manikde!(me[2:2,:], ContinuousScalar);manikde!(me0[2:2,:], ContinuousScalar)], c=["red";"blue"], legend=["pred";"meas"])
+
+  push!(hdl, pc)
+  push!(hdl, pcl)
+  push!(hdl, pl)
+
+  hstack(vstack(pc,pcl), pl)
+end
+
+
 """
     $SIGNATURES
 
-Return plot of each specific factor.
+Calculate the "inverse" SLAM solution to compare measured and predicted noise model samples.
 """
-function plotFactor(dfg::G, fctsym::Symbol; hdl=[]) where G <: AbstractDFG
-  plotFactor(dfg, fctsym, getFactorType(dfg, fctsym), hdl=hdl)
+function plotFactorMeasurements(dfg::AbstractDFG, fctsym::Symbol;hdl=[])
+  fct = getFactorType(dfg, fctsym)
+  plotFactorMeasurements(dfg, fctsym, fct, hdl=hdl)
 end
 
-function plotFactor(dfg::G, fctsym::Symbol, fct::Pose2Point2Range; hdl=[]) where G <: AbstractDFG
+
+
+function plotFactor(dfg::AbstractDFG, fctsym::Symbol, fct::Pose2Point2Range; hdl=[])
   # variables
   vars = ls(dfg, fctsym)
 
@@ -46,7 +87,16 @@ function plotFactor(dfg::G, fctsym::Symbol, fct::Pose2Point2Range; hdl=[]) where
   addFactor!(tfg, [pose;poin], fct, autoinit=false)
   manualinit!(tfg, pose, getKDE(dfg,pose))
   manualinit!(tfg, poin, getKDE(dfg,poin))
-  apts = approxConv(tfg, ls(tfg,pose)[1], poin)
+  apts=Array{Float64,2}(undef, 2, 0)
+  loop =true
+  while loop
+    try
+      apts = approxConv(tfg, ls(tfg,pose)[1], poin)
+      loop = false
+    catch
+      @warn "plotFactor Pose2Point2BearingRange non-linear solve fail on approxConv, retrying"
+    end
+  end
   plhist2 = Gadfly.plot(x=apts[1,:], y=apts[2,:], Geom.histogram2d)
   spc = mean(smps)
   plt = drawPosesLandms(tfg, point_size=5pt, spscale=0.2*spc)
@@ -138,9 +188,7 @@ function plotFactor(dfg::AbstractDFG, fctsym::Symbol, fct::Pose2Point2Bearing; h
 end
 
 
-
-
-function plotFactor(dfg::AbstractDFG, fctsym::Symbol, fct::Pose2Point2BearingRange; hdl = [])
+function plotFactor(dfg::AbstractDFG, fctsym::Symbol, fct::Pose2Point2BearingRange; hdl=[])
     #
     hdlb = []
 
@@ -184,11 +232,70 @@ function plotFactor(dfg::AbstractDFG, fctsym::Symbol, fct::Pose2Point2BearingRan
 
     push!(hdl, predLandm)
 
-    return hstack(vstack(hdl[1],hdl[2],hdl[3]),vstack(hdl[4],hdl[5],hdl[6]),vstack(predLandm, hdlb[5],hdl[7]))
+    # measurement solution
+    plotFactorMeasurements(dfg, fctsym, hdl=hdl)
+
+    return hstack(vstack(hdl[1],hdl[2],hdl[3]),vstack(hdl[4],hdl[5],hdl[6]),vstack(predLandm, hdlb[5],hdl[7]), vstack(hdl[14],hdl[15],hdl[16]))
+end
+
+
+function plotFactor(dfg::AbstractDFG, fctsym::Symbol, fct::Pose2Pose2; hdl=[])
+
+  # variables
+  fct = getFactor(dfg, fctsym)
+  vars = fct._variableOrderSymbols
+
+  pv1 = plotPose(dfg, vars[1], hdl=hdl)
+  pv2 = plotPose(dfg, vars[2], hdl=hdl)
+
+  pv12 = plotFactorMeasurements(dfg, fctsym, hdl=hdl)
+
+  vstack(pv1, pv2, pv12)
+end
+
+
+"""
+    $SIGNATURES
+
+Return plot of each specific factor.
+"""
+function plotFactor(dfg::AbstractDFG, fctsym::Symbol; hdl=[])
+  plotFactor(dfg, fctsym, getFactorType(dfg, fctsym), hdl=hdl)
 end
 
 
 
 
 
+## Reports
+
+
+function reportFactors(dfg::AbstractDFG,
+                       T::Union{Type{Pose2Pose2}, Type{Pose2Point2BearingRange}, Type{Pose2Point2Range}, Type{Pose2Point2Bearing}},
+                       fcts::Vector{Symbol}=ls(dfg, T);
+                       filepath="/tmp/caesar/random/report/$T.pdf",
+                       show::Bool=true  )
+  #
+  ss = split(filepath, '/')
+  path = joinpath(ss[1:(end-1)]...)
+  mkpath(path)
+
+  files = String[]
+  for fc in fcts
+    file = joinpath(path,"$fc.pdf")
+    plotFactor(dfg, fc) |> PDF(file)
+    push!(files, file)
+  end
+  push!(files, filepath)
+
+  2 < length(files) ? run(`pdfunite $files`) : nothing
+  !show ? nothing : (@async run(`evince $filepath`))
+  return filepath
+end
+
+
+
+
+
+
 #

src/RoMEPlotting.jl

@@ -85,7 +85,9 @@ export
   plotProductVsKDE,
   plotVariableGivenFactor,
   plotCliqDownMsgs,
-  plotFactor
+  plotFactor,
+  plotFactorMeasurements,
+  reportFactors
 
 
 

src/RobotViz.jl

@@ -262,9 +262,9 @@ function drawPosesLandms(fgl::AbstractDFG;
   xmin != nothing && xmax != nothing && xmin == xmax ? error("xmin must be less than xmax") : nothing
   ymin != nothing && ymax != nothing && ymin == ymax ? error("ymin must be less than ymax") : nothing
   ll = getVariableIds(fgl, regexLandmark)
-  p = drawPoses(fgl, from=from,to=to,meanmax=meanmax,lbls=lbls,drawhist=drawhist, spscale=spscale)
+  p = drawPoses(fgl, from=from,to=to,meanmax=meanmax,lbls=lbls,drawhist=drawhist, spscale=spscale, contour=contour)
   if length(ll) > 0
-    pl = drawLandms(fgl, from=from, to=to, minnei=minnei,lbls=lbls,drawhist=drawhist, MM=MM, showmm=showmm, point_size=point_size)
+    pl = drawLandms(fgl, from=from, to=to, minnei=minnei,lbls=lbls,drawhist=drawhist, MM=MM, showmm=showmm, point_size=point_size, contour=contour)
     for l in pl.layers
       push!(p.layers, l)
     end
@@ -364,7 +364,8 @@ function plotPose(pt::Pose2,
                   c=nothing,
                   axis=nothing,
                   scale::Float64=0.2,
-                  overlay=nothing  )
+                  overlay=nothing,
+                  hdl=[]  )
   #
   # ops = buildHybridManifoldCallbacks(pt.manifolds)
   # @show ran = getKDERange(p, addop=ops[1], diffop=ops[2])
@@ -387,6 +388,9 @@ function plotPose(pt::Pose2,
   # deal with overlay
 
 
+  push!(hdl, p1)
+  push!(hdl, p2)
+
   Gadfly.hstack(p1,p2)
 end
 
@@ -398,27 +402,53 @@ function plotPose(pt::Pose2,
                   c=nothing,
                   axis=nothing,
                   scale::Float64=0.2,
-                  overlay=nothing )
+                  overlay=nothing,
+                  hdl=[]  )
   #
-  plotPose(pt, [pp;],title,levels=levels,c=c,axis=axis,scale=scale, overlay=overlay)
+  plotPose(pt, [pp;],title,levels=levels,c=c,axis=axis,scale=scale, overlay=overlay, hdl=hdl )
 end
 
 
-function plotPose(::DynPose2, bels::Vector{BallTreeDensity}, title; levels::Int=5, c=nothing, axis=nothing)
+function plotPose(::DynPose2,
+                  bels::Vector{BallTreeDensity},
+                  title;
+                  levels::Int=5,
+                  c=nothing,
+                  axis=nothing,
+                  hdl=[] )
+  #
   p1 = plotKDE(bels, dims=[1;2], levels=levels, c=c, title=title)
   p2 = plotKDE(bels, dims=[3], c=c)
   p3 = plotKDE(bels, dims=[4;5], levels=levels, c=c)
+
+  push!(hdl, p1)
+  push!(hdl, p2)
+  push!(hdl, p3)
+
   Gadfly.vstack(p1,p2,p3)
 end
 
 # import RoMEPlotting: plotPose
 
-function plotPose(::Pose3, bels::Vector{BallTreeDensity}, title; levels::Int=5, c=nothing, axis=nothing)
+function plotPose(::Pose3,
+                  bels::Vector{BallTreeDensity},
+                  title;
+                  levels::Int=5,
+                  c=nothing,
+                  axis=nothing,
+                  hdl=[]  )
+  #
   @show title
   p1 = plotKDE(bels, dims=[1;2], levels=levels, c=c, title=title)
   p2 = plotKDE(bels, dims=[3], c=c)
   p3 = plotKDE(bels, dims=[4;5], levels=levels, c=c)
   p4 = plotKDE(bels, dims=[6], c=c)
+
+  push!(hdl, p1)
+  push!(hdl, p2)
+  push!(hdl, p3)
+  push!(hdl, p4)
+
   Gadfly.vstack(p1,p2,p3,p4)
 end
 
@@ -435,12 +465,13 @@ function plotPose(fgl::G,
                   scale::Float64=0.2,
                   show::Bool=false,
                   filepath::AS="/tmp/tempposeplot.svg",
-                  app::AS="eog" ) where {G <: AbstractDFG, AS <: AbstractString}
+                  app::AS="eog",
+                  hdl=[]  ) where {G <: AbstractDFG, AS <: AbstractString}
   #
   typ = getData(getVariable(fgl, syms[1])).softtype
   pt = string(string.(syms)...)
   getvertsgg = (sym) -> getKDE(getVariable(fgl, sym))
-  pl = plotPose(typ, getvertsgg.(syms), pt, levels=levels, c=c, axis=axis, scale=scale)
+  pl = plotPose(typ, getvertsgg.(syms), pt, levels=levels, c=c, axis=axis, scale=scale, hdl=hdl )
 
   if length(filepath) > 0
     ext = split(filepath, '.')[end]
@@ -466,9 +497,10 @@ function plotPose(fgl::G,
                   scale::Float64=0.2,
                   show::Bool=false,
                   filepath::AS="/tmp/tempposeplot.svg",
-                  app::AS="eog" ) where {G <: AbstractDFG, AS <: AbstractString}
+                  app::AS="eog",
+                  hdl=[]  ) where {G <: AbstractDFG, AS <: AbstractString}
   #
-  plotPose(fgl, [sym;], levels=levels, axis=axis, show=show, filepath=filepath, app=app)
+  plotPose(fgl, [sym;], levels=levels, axis=axis, show=show, filepath=filepath, app=app, hdl=hdl )
 end
 
 # deprecated