1
"""
2
    $(SIGNATURES)
3
Initialize empty visualizer
4
"""
5
function initVis(;showLocal::Bool = true)
6 0
    vis = Visualizer()
7 0
    showLocal && open(vis)
8 0
    return vis
9
end
10

11

12

13
"""
14
    $(SIGNATURES)
15
Initialize empty visualizer
16
"""
17
function initBotVis2(;showLocal::Bool = true)::BotVis2
18 0
    vis = Visualizer()
19 0
    showLocal && open(vis)
20 0
    return BotVis2(vis,
21
                   # Dict{Symbol, NTuple{3,Float64}}(),
22
                   # Dict{Symbol, NTuple{3,Float64}}(),
23
                   Dict{Symbol, Tuple{Symbol, Vector{Bool}, Vector{Float64}}}() )
24
end
25

26

27
"""
28
    $(SIGNATURES)
29
Draw all poses in an 2d factor graph, use meanmax = :max or :mean for distribution max or mean, respectively.
30
"""
31
function drawPoses2!(botvis::BotVis2,
32
                     fgl::FactorGraph;
33
                     meanmax::Symbol=:max,
34
                     triadLength=0.25,
35
                     sessionId::String="Session" )::Nothing
36
    #
37 0
    xx, ll = Caesar.ls(fgl)
38

39 0
    for x in xx
40 0
        X = getKDE(fgl, x)
41 0
        xmx = meanmax == :max ? getKDEMax(X) : getKDEMean(X)
42 0
        trans = Translation(xmx[1:2]..., 0.0)  LinearMap(RotZ(xmx[3]))
43 0
        if !haskey(botvis.cachevars, x)
44 0
            triad = Triad(triadLength)
45 0
            setobject!(botvis.vis[Symbol(sessionId)][:poses][x], triad)
46 0
            botvis.cachevars[x] = (:Pose2, [true;], xmx)
47
        else
48 0
            botvis.cachevars[x][3][:] = xmx
49
        end
50 0
        settransform!(botvis.vis[Symbol(sessionId)][:poses][x], trans)
51
    end
52 0
	return nothing
53
end
54

55
"""
56
    $(SIGNATURES)
57
Draw all landmarks in an 2d factor graph, use meanmax = :max or :mean for distribution max or mean, respectively.
58
"""
59
function drawLandmarks2!(botvis::BotVis2,
60
                         fgl::FactorGraph;
61
                         meanmax::Symbol=:max,
62
                         sessionId::String="Session"  )::Nothing
63
    #
64 0
    xx, ll = Caesar.ls(fgl)
65

66 0
    for x in ll
67 0
        X = getKDE(fgl, x)
68 0
        xmx = meanmax == :max ? getKDEMax(X) : getKDEMean(X)
69 0
        trans = Translation(xmx[1:2]..., 0.0)
70 0
        if !haskey(botvis.cachevars, x)
71 0
            setobject!(botvis.vis[Symbol(sessionId)][:landmarks][x], lmpoint, greenMat)
72 0
            botvis.cachevars[x] = (:Point2, [true;], [xmx[1]; xmx[2]; 0.0 ] )
73
        else
74 0
            botvis.cachevars[x][3][1:2] = xmx[1:2]
75
        end
76 0
        settransform!(botvis.vis[Symbol(sessionId)][:landmarks][x], trans)
77
    end
78 0
	return nothing
79
end
80

81

82

83

84

85
#

Read our documentation on viewing source code .

Loading