Merge pull request #109 from JuliaRobotics/22Q4/enh/plotbox

Box plot tools

Project.toml

@@ -8,15 +8,19 @@ Caesar = "62eebf14-49bc-5f46-9df9-f7b7ef379406"
 ColorSchemes = "35d6a980-a343-548e-a6ea-1d62b119f2f4"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a"
+GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326"
 Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 
 [compat]
 Caesar = "0.14"
 ColorSchemes = "3"
 DocStringExtensions = "0.8, 0.9"
 GLMakie = "0.7, 0.8"
+GeometryBasics = "0.4"
 Manifolds = "0.8.15"
+StaticArrays = "1"
 TensorCast = "0.4"
 julia = "1.8"
 

src/Arena.jl

@@ -2,12 +2,14 @@ module Arena
 
 # using Colors
 import Manifolds as MJL
+import GeometryBasics as GeoB
 using Caesar
 const _PCL = Caesar._PCL
 # can switch to WGLMakie after https://github.com/SimonDanisch/JSServe.jl/issues/131
 using GLMakie
 using ColorSchemes
 using TensorCast
+using StaticArrays
 
 # a lot of legacy code has been moved to the attic
 

src/Exports.jl

@@ -1,4 +1,5 @@
 
 export plotPointCloud2D
 export plotPointCloud, plotPointCloudPair
-export plotGraphPointClouds
+export plotGraphPointClouds
+export plotBoundingBox, plotBoundingBox!

src/services/PlotPointCloudMap.jl

@@ -67,7 +67,8 @@ function plotGraphPointClouds(
   dfg::AbstractDFG,
   getpointcloud::Function = (v)->_PCL.getDataPointCloud(dfg, v, Regex("PCLPointCloud2"); checkhash=false);
   varList = (listVariables(dfg) |> sortDFG .|> string),
-  solveKey = :default
+  solveKey = :default,
+  ax=nothing
 )
   pl = nothing
 
@@ -89,6 +90,7 @@ function plotGraphPointClouds(
   M = getManifold(Pose3)
   ϵ0_ = MJL.identity_element(M)
   ϵ0 = ArrayPartition(ϵ0_.parts...)
+  pc_map = _PCL.PointCloud()
   for vl in varList
     @show vl
     count += 1
@@ -114,22 +116,68 @@ function plotGraphPointClouds(
     wPp = MJL.exp(M,ϵ0,MJL.hat(M,ϵ0,w_Cwp))
     # wPp = getSolverData(v, solveKey).val[1]
     wPC = Caesar._PCL.apply(M, wPp, pc)
-
-    if pl isa Nothing
-      pl = plotPointCloud(wPC; plotfnc = scatter, col=-1.0, ax=nothing)
-      pc_map = deepcopy(wPC)
+
+
+    if isnothing(pl) && isnothing(ax)
+      pl = plotPointCloud(wPC; plotfnc = scatter, col=-1.0, ax)
+      # pc_map = deepcopy(wPC)
     else
       # col = -1*(count%2)
       col = if vl == varList[end]
         pc_last = wPC
         0
       else
-        pc_map = cat(pc_map, wPC; reuse=true)
+        # pc_map = cat(pc_map, wPC; reuse=true)
         -1
       end
-      plotPointCloud(wPC; plotfnc = scatter!) #, col)
+      plotPointCloud(wPC; plotfnc = scatter!, ax) #, col)
     end
+    cat(pc_map, wPC; reuse=true)
   end
 
   return pl, pc_map, pc_last
+end
+
+
+function plotBoundingBox!(
+  ax,
+  BB::_PCL.AbstractBoundingBox;
+  color=:red
+)
+  _line3d!(ax, a, b; _color=color ) = lines!(ax, [b[1];a[1]], [b[2];a[2]], [b[3];a[3]]; color=_color  )
+
+  corners = _PCL.getCorners(BB)
+
+  # https://math.stackexchange.com/questions/1472049/check-if-a-point-is-inside-a-rectangular-shaped-area-3d
+  _line3d!(ax, corners[1], corners[2])
+  _line3d!(ax, corners[1], corners[4])
+  _line3d!(ax, corners[2], corners[3])
+  _line3d!(ax, corners[4], corners[3])
+
+  _line3d!(ax, corners[5], corners[6])
+  _line3d!(ax, corners[5], corners[8])
+  _line3d!(ax, corners[6], corners[7])
+  _line3d!(ax, corners[8], corners[7])
+
+  _line3d!(ax, corners[1], corners[5])
+  _line3d!(ax, corners[2], corners[6])
+  _line3d!(ax, corners[3], corners[7])
+  _line3d!(ax, corners[4], corners[8])
+
+  if BB isa _PCL.OrientedBoundingBox
+    _line3d!(ax, [0;0;0.], corners[1]; _color=:gray)
+  end
+
+  nothing
+end
+
+function plotBoundingBox(
+  BB::_PCL.AbstractBoundingBox,
+)
+  # x,y,zz = makeWireframeHyperRectangle(BB)
+  # wireframe(x,y,zz)
+  fig = Figure()
+  ax = Axis3(fig[1,1])
+  plotBoundingBox!(ax, BB)
+  fig
 end