1

2

3

4
function visualizeDensityMesh!(vc,
5
                               fgl::FactorGraph,
6
                               lbl::Symbol;
7
                               levels=3,
8
                               meshid::Int=2  )
9

10 0
  pl1 = marginal(getVertKDE(fgl,lbl),[1;2;3])
11

12 0
  gg = (x, a=0.0) -> evaluateDualTree(pl1, [x[1] x[2] x[3]]')[1]-a  #([x[1];x[2];x[3]]')'
13

14 0
  x = getKDEMax(pl1)
15 0
  maxval = gg(x)
16

17 0
  vv = getKDERange(pl1)
18 0
  lower_bound = Vec(vec(vv[:,1])...)
19 0
  upper_bound = Vec(vec(vv[:,2])...)
20

21 0
  levels = linspace(0.0,maxval,levels+2)
22

23
  # MD = []
24 0
  for val in levels[2:(end-1)]
25 0
    meshdata = GeometryData(contour_mesh(x -> gg(x,val), lower_bound, upper_bound))
26 0
    meshdata.color = RGBA( val/(1.5*maxval),0.0,1.0,val/(1.5*maxval))
27
    # push!(MD, meshdata)
28 0
    setgeometry!(vc[:meshes][lbl][Symbol("lev$(val)")], meshdata)
29
  end
30
  # mptr = Any(MD)
31
  # vc.meshes[lbl] = mptr
32
  # Visualizer(mptr, meshid) # meshdata
33 0
  nothing
34
end
35

36

37

38
function drawgt!(vc,
39
                 sym::Symbol,
40
                 gtval::Tuple{Symbol, Vector{Float64}};
41
                 session::AbstractString="NA"  )
42
  #
43 0
  if gtval[1] == :XYZ
44 0
    drawpoint!(vc, sym, tf=Translation(gtval[2][1],gtval[2][2],gtval[2][3]),
45
          session=session,
46
          color=RGBA(1.0,0,0,0.5),
47
          collection=:gt_landm  )
48 0
  elseif gtval[1] == :XYZqWXYZ
49 0
    drawpose!(vc, sym,
50
          tf = Translation(gtval[2][1],gtval[2][2],gtval[2][3]) 
51
               LinearMap(CoordinateTransformations.Quat(gtval[2][4],gtval[2][5],gtval[2][6],gtval[2][7])),
52
          session=session,
53
          collection=:gt_poses  )
54
  else
55 0
    warn("unknown ground truth drawing type $(gtval[1])")
56
  end
57

58 0
  nothing
59
end
60

61
# TODO -- maybe we need RemoteFactorGraph type
62
function visualizeallposes!(vc,
63
                            fgl::FactorGraph;
64
                            drawlandms::Bool=true,
65
                            drawtype::Symbol=:max,
66
                            gt::Dict{Symbol, Tuple{Symbol,Vector{Float64}}}=Dict{Symbol, Tuple{Symbol,Vector{Float64}}}(),
67
                            api::DataLayerAPI=localapi  )
68
  #
69 0
  session = fgl.sessionname
70 0
  topoint = gettopoint(drawtype)
71

72 0
  dotwo = false
73 0
  dothree = false
74 0
  po,ll = ls(fgl)
75 0
  if length(po) > 0
76 0
    sym = po[1]
77 0
    X = getVal(fgl, sym, api=api )
78 0
    dotwo, dothree = getdotwothree(sym, X)
79
  end
80

81
  # TODO -- move calls higher in abstraction to be more similar to drawdbdirector()
82 0
  for p in po
83 0
    vert = getVert(fgl, p, api=api )
84 0
    drawpose!(vc, vert, topoint, dotwo, dothree, session=session)
85 0
    if haskey(gt, p)
86 0
      drawgt!(vc, p, gt[p], session=session)
87
    end
88
  end
89 0
  if drawlandms
90 0
    for l in ll
91 0
      den = getVertKDE(fgl, l, api=api)
92 0
      pointval = topoint(den)
93 0
      drawpoint!(vc, l, tf=Translation(pointval[1:3]...), session=session)
94 0
      if haskey(gt, l)
95 0
        drawgt!(vc, l, gt[l], session=session)
96
      end
97
    end
98
  end
99

100 0
  nothing
101
end
102

103

104

105
function drawposepoints!(vis,
106
                         vert::Graphs.ExVertex;
107
                         session::AbstractString="NA"  )
108
  #
109 0
  vsym = Symbol(vert.label)
110 0
  X = getVal(vert)
111

112 0
  dotwo, dothree = getdotwothree(vsym, X)
113 0
  makefromX = (X::Array{Float64,2}, i::Int) -> X[1:3,i]
114 0
  if dotwo
115 0
    makefromX = (X::Array{Float64,2}, i::Int) -> Float64[X[1:2,i];0.0]
116
  end
117

118 0
  XX = Vector{Vector{Float64}}()
119 0
  for i in 1:size(X,2)
120 0
    push!(XX, makefromX(X,i))
121
  end
122 0
  pointcloud = PointCloud(XX)
123 0
  if string(vsym)[1] == 'l'
124 0
    pointcloud.channels[:rgb] = [RGB(1.0, 1.0, 0) for i in 1:length(XX)]
125 0
  elseif string(vsym)[1] == 'x'
126 0
    pointcloud.channels[:rgb] = [colorwheel(vert.index) for i in 1:length(XX)]
127
  end
128 0
  setgeometry!(vis[Symbol(session)][:posepts][vsym], pointcloud)
129 0
  nothing
130
end
131

132
function drawposepoints!(vis,
133
      fgl::FactorGraph,
134
      sym::Symbol;
135
      session::AbstractString="NA",
136
      api::DataLayerAPI=dlapi  )
137
  #
138 0
  vert = getVert(fgl, sym, api=api)
139 0
  drawposepoints!(vis, vert, session=session, api=localapi) # definitely use localapi
140 0
  nothing
141
end
142

143

144

145
function deletemeshes!(vc)
146 0
  delete!(vc[:meshes])
147
end
148

149

150

151
"""
152
    $(SIGNATURES)
153

154
Draw a line segment between to vertices.
155
"""
156
function drawLineBetween!(vis,
157
                          session::AbstractString,
158
                          fr::Graphs.ExVertex,
159
                          to::Graphs.ExVertex;
160
                          scale=0.01,
161
                          name::Symbol=:edges,
162
                          subname::Union{Nothing,Symbol}=nothing,
163
                          color=RGBA(0,1.0,0,0.5)   )
164
  #
165 0
  dotwo, dothree = getdotwothree(Symbol(fr.label), getVal(fr))
166

167 0
  xipt = zeros(3); xjpt = zeros(3);
168 0
  if dothree
169 0
    xi = marginal(getVertKDE( fr ),[1;2;3] )
170 0
    xj = marginal(getVertKDE( to ),[1;2;3] )
171 0
    xipt[1:3] = getKDEMax(xi)
172 0
    xjpt[1:3] = getKDEMax(xj)
173 0
  elseif dotwo
174 0
    xi = marginal(getVertKDE( fr ),[1;2] )
175 0
    xj = marginal(getVertKDE( to ),[1;2] )
176 0
    xipt[1:2] = getKDEMax(xi)
177 0
    xjpt[1:2] = getKDEMax(xj)
178
  end
179

180 0
  lbl = Symbol(string(fr.label,to.label))
181 0
  place = vis[Symbol(session)][name][lbl]
182 0
  if subname != nothing
183 0
    place = vis[Symbol(session)][name][subname][lbl]
184
  end
185 0
  drawLine!(place, xipt, xjpt, color=color, scale=scale )
186 0
  nothing
187
end
188

189

190
"""
191
    $(SIGNATURES)
192

193
Draw a line segment between to nodes in the factor graph.
194
"""
195
function drawLineBetween!(vis,
196
                          fgl::FactorGraph,
197
                          fr::Symbol,
198
                          to::Symbol;
199
                          scale=0.01,
200
                          name::Symbol=:edges,
201
                          subname::Union{Nothing,Symbol}=nothing,
202
                          color=RGBA(0,1.0,0,0.5),
203
                          api::DataLayerAPI=dlapi  )
204
  #
205 0
  v1 = getVert(fgl, fr, api=api)
206 0
  v2 = getVert(fgl, to, api=api)
207

208 0
  drawLineBetween!(vis,fgl.sessionname, v1,v2,scale=scale,name=name,subname=subname,color=color   )
209
end
210

211

212
"""
213
    $(SIGNATURES)
214

215
Assume odometry chain and draw edges between subsequent poses. Use keyword arguments to change colors, etc.
216
"""
217
function drawAllOdometryEdges!(vis,
218
                               fgl::FactorGraph;
219
                               scale=0.01,
220
                               name::Symbol=:edges,
221
                               color=RGBA(0,1.0,0,0.5),
222
                               api::DataLayerAPI=dlapi  )
223
  #
224 0
  xx, ll = ls(fgl)
225

226 0
  for i in 1:(length(xx)-1)
227 0
    drawLineBetween!(vis, fgl, xx[i],xx[i+1], api=api , color=color, scale=scale, name=name )
228
  end
229

230 0
  nothing
231
end
232

233

234
function drawAllBinaryFactorEdges!(vis,
235
                                   fgl::FactorGraph;
236
                                   scale=0.01,
237
                                   api::DataLayerAPI=dlapi  )
238
  #
239 0
  sloth = findAllBinaryFactors(fgl, api=api)
240

241 0
  for (teeth, toe) in sloth
242 0
    color = pointToColor(toe[3])
243 0
    drawLineBetween!(vis, fgl, toe[1], toe[2], subname=toe[3], scale=scale, color=color)
244
  end
245 0
  nothing
246
end
247

248

249

250
"""
251
    $(SIGNATURES)
252

253
Update all triads listed in poseswithdepth[Symbol(vert.label)] with wTb. Prevents cycles in
254
remote tree viewer of Visualizer.
255
"""
256
function updateparallelposes!(vis,
257
                              vert::Graphs.ExVertex,
258
                              poseswithdepth::Dict;
259
                              wTb::CoordinateTransformations.AbstractAffineMap=
260
                                    Translation(0,0,0.0)  LinearMap(
261
                                    CoordinateTransformations.Quat(1.0, 0, 0, 0))    )
262
  #
263

264 0
  if haskey(poseswithdepth, Symbol(vert.label))
265 0
    for cp in poseswithdepth[Symbol(vert.label)]
266 0
      settransform!(cp, wTb)
267
    end
268
  end
269 0
  nothing
270
end
271

272

273

274

275
function drawdbdirector(;addrdict::NothingUnion{Dict{AbstractString, AbstractString}}=nothing)
276
  # Uncomment out for command line operation
277 0
  cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict,drawdepth=true, drawedges=true)
278 0
  session = addrdict["session"]
279

280 0
  poseswithdepth = Dict()
281
  # poseswithdepth[:x1] = 0 # skip this pose -- there is no big data before ICRA
282

283 0
  vis = startDefaultVisualization()
284 0
  sleep(1.0)
285

286 0
  param = Dict()
287 0
  try
288 0
    param = robotsetup(cloudGraph, session)
289
  catch
290 0
    warn("No robot parameters found for the session, continuing with basic visualization only")
291
  end
292

293 0
  drawloop = Bool[true]
294 0
  println("Starting draw loop...")
295 0
  while drawloop[1]
296 0
    drawdbsession(vis, cloudGraph, addrdict, param, poseswithdepth) #,  db[collection]
297 0
    println(".")
298 0
    sleep(0.5)
299
  end
300

301 0
  println("Finishing askdrawdirectordb.")
302 0
  nothing
303
end
304

305

306
#syntax support lost in 0.5, but see conversation at
307
# function (dmdl::DrawObject)(vc::VisualizationContainer)
308
# issue  https://github.com/JuliaLang/julia/issues/14919

Read our documentation on viewing source code .

Loading