From ab114a0fb90b0849fa4bbc3688e99f2f19cf15d9 Mon Sep 17 00:00:00 2001
From: dehann <dehann.fourie@gmail.com>
Date: Thu, 8 Aug 2024 00:49:26 -0700
Subject: [PATCH 1/2] add plotGraph3d

---
 Project.toml                     |  2 +-
 src/Arena.jl                     |  6 +++-
 src/services/PlotGraphGeneric.jl | 52 ++++++++++++++++++++++++++++++++
 3 files changed, 58 insertions(+), 2 deletions(-)
 create mode 100644 src/services/PlotGraphGeneric.jl

diff --git a/Project.toml b/Project.toml
index ee2c5b6..afe0dbf 100644
--- a/Project.toml
+++ b/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"
diff --git a/src/Arena.jl b/src/Arena.jl
index fc0924c..9ce6620 100644
--- a/src/Arena.jl
+++ b/src/Arena.jl
@@ -22,8 +22,9 @@ 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 plotGraph3d
 
 # include("Exports.jl")
 # include("services/PlotBoundingBox.jl")
@@ -33,4 +34,7 @@ include("services/PlotHistogramGrid.jl")
 # support weakdeps exports
 include("../ext/Prototypes.jl")
 
+include("services/PlotGraphGeneric.jl")
+
+
 end # module
diff --git a/src/services/PlotGraphGeneric.jl b/src/services/PlotGraphGeneric.jl
new file mode 100644
index 0000000..873235b
--- /dev/null
+++ b/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
\ No newline at end of file

From 9177c8a7ab12eb737e0fa0d721dcf6955c7d26dd Mon Sep 17 00:00:00 2001
From: dehann <dehann.fourie@gmail.com>
Date: Thu, 8 Aug 2024 01:14:09 -0700
Subject: [PATCH 2/2] add plotPoints for SO3

---
 src/Arena.jl                  |  2 ++
 src/services/PlotManifolds.jl | 22 ++++++++++++++++++++++
 2 files changed, 24 insertions(+)
 create mode 100644 src/services/PlotManifolds.jl

diff --git a/src/Arena.jl b/src/Arena.jl
index 9ce6620..d05485e 100644
--- a/src/Arena.jl
+++ b/src/Arena.jl
@@ -24,9 +24,11 @@ using DocStringExtensions
 
 # NOTE 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")
diff --git a/src/services/PlotManifolds.jl b/src/services/PlotManifolds.jl
new file mode 100644
index 0000000..469050e
--- /dev/null
+++ b/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
\ No newline at end of file