Merge pull request #124 from JuliaRobotics/24Q3/enh/plotgraph3d

add plotGraph3d

Project.toml

@@ -29,7 +29,7 @@ Caesar = "0.16"
 ColorSchemes = "3"
 Colors = "0.12"
 DelimitedFiles = "1"
-DistributedFactorGraphs = "0.23"
+DistributedFactorGraphs = "0.23, 0.24"
 DocStringExtensions = "0.9"
 Downloads = "1"
 GLMakie = "0.9, 0.10"

src/Arena.jl

@@ -22,15 +22,21 @@ using StaticArrays
 using ProgressMeter
 using DocStringExtensions
 
+# NOTE a lot of legacy code has been moved to the attic
 
-# a lot of legacy code has been moved to the attic
+export plotPoints
+export plotGraph3d
 
 # include("Exports.jl")
+include("services/PlotManifolds.jl")
 # include("services/PlotBoundingBox.jl")
 include("services/PlotFeatureTracks.jl")
 include("services/PlotHistogramGrid.jl")
 
 # support weakdeps exports
 include("../ext/Prototypes.jl")
 
+include("services/PlotGraphGeneric.jl")
+
+
 end # module

src/services/PlotGraphGeneric.jl

@@ -0,0 +1,52 @@
+
+
+
+function plotGraph3d(
+  dfg::AbstractDFG;
+  tag = :POSE,
+  labels::AbstractVector{Symbol} = sortDFG(ls(dfg; tags=[tag;])), # FIXME, better support for multiple trajectories via tags
+  solveKey = :default,
+  title::AbstractString = string(
+    getSessionLabel(dfg), 
+    ",  solveKey: ", solveKey,
+    ",  (", length(labels), ")",
+    "\n", getTimestamp(dfg[labels[1]]),
+    " --> ", getTimestamp(dfg[labels[end]]),
+  ),
+  drawTrajectory::Bool=true,
+  drawTrajectoryMarkers::Bool = drawTrajectory,
+  fig = Figure(),
+)
+  #
+
+  _getppepos(_v::DFGVariable{<:Position{2}}, solvK) = [getPPESuggested(_v, solvK)[1:2]; 0.0]
+  _getppepos(_v::DFGVariable{<:Pose2}, solvK) = [getPPESuggested(_v, solvK)[1:2]; 0.0]
+  _getppepos(_v::DFGVariable{<:Position{3}}, solvK) = getPPESuggested(_v, solvK)[1:3]
+  _getppepos(_v::DFGVariable{<:Pose3}, solvK) = getPPESuggested(_v, solvK)[1:3]
+  _getppepos(_v::DFGVariable{<:RoME.RotVelPos}, solvK) = getPPESuggested(_v, solvK)[7:9]
+
+  ax1 = Axis3(fig[1, 1]; title)
+  # Axis(f[2, 1], title = L"\sum_i{x_i \times y_i}")
+  # Axis(f[3, 1], title = rich(
+  #     "Rich text title",
+  #     subscript(" with subscript", color = :slategray)
+  # ))
+
+  _ppes = _getppepos.(getVariable.(dfg, labels), solveKey) # getPPESuggested.(dfg, labels, solveKey)
+
+  @cast ppes[i,d] := _ppes[i][d]
+
+  scatter!(ax1, ppes[:,1],ppes[:,2],ppes[:,3],)
+  if drawTrajectory
+    lines!(ax1, ppes[:,1],ppes[:,2],ppes[:,3],)
+  end
+
+  if drawTrajectoryMarkers
+    scatter!(ax1, Point3f(ppes[1,1:3]...), markersize = 20, markerspace = :pixel, marker = '✪', label = "traj start")
+    scatter!(ax1, Point3f(ppes[end,1:3]...), markersize = 20, markerspace = :pixel, marker = '⊙', label = "traj end")
+
+    axislegend(ax1)
+  end
+
+  fig
+end

src/services/PlotManifolds.jl

@@ -0,0 +1,22 @@
+
+
+
+function plotPoints(
+  ::typeof(SpecialOrthogonal(3)),
+  ps::AbstractVector{<:AbstractMatrix{<:Real}};
+  color = :red
+)
+  scene = Scene()
+  cam3d!(scene)
+  wireframe!(scene, Makie.Sphere( Point3f(0), 1.0), color=:gray)
+  Makie.scale!(scene, 1.0, 1.0, 1.0)
+  Makie.rotate!(scene, Vec3f(1, 0, 0), 0.5) # 0.5 rad around the y axis
+
+
+  for R in ps
+    vX = R*SA[1;0;0.0]
+    scatter!( scene, Point3f(vX...); color )
+  end
+
+  scene
+end