diff --git a/src/Arena.jl b/src/Arena.jl index 663109d..0ff3cd6 100644 --- a/src/Arena.jl +++ b/src/Arena.jl @@ -25,7 +25,7 @@ using DocStringExtensions # NOTE a lot of legacy code has been moved to the attic export plotPoints -export plotGraph3d +export plot3d! # include("Exports.jl") include("services/PlotManifolds.jl") @@ -39,5 +39,7 @@ include("../ext/Prototypes.jl") include("services/TodoConsolidate.jl") include("services/PlotGraphGeneric.jl") +include("Deprecated.jl") + end # module diff --git a/src/Deprecated.jl b/src/Deprecated.jl new file mode 100644 index 0000000..8f957aa --- /dev/null +++ b/src/Deprecated.jl @@ -0,0 +1,92 @@ + + +@deprecate plotGraph3d(w...;kw...) plot3d!(w...;kw...) + +@deprecate plot3d(w...;kw...) plot3d!(w...;kw...) + + +# function plot3d!(fg; +# linewidth = 0.025, +# lengthscale=0.15f0, +# arrowsize = Vec3f(0.05, 0.05, 0.1), +# solveKey=:parametric, +# vsyms = ls(fg, r"^x"), +# ) + + +# ps = map(enumerate(vsyms)) do (i,v) +# val = getVal(fg, v; solveKey)[1] +# Point3f(val[1:3]) +# end + +# nxs = map(vsyms) do v +# val = getVal(fg, v; solveKey)[1] +# Point3f(val.x[2][:,1]) +# end +# nys = map(vsyms) do v +# val = getVal(fg, v; solveKey)[1] +# Point3f(val.x[2][:,2]) +# end +# nzs = map(vsyms) do v +# val = getVal(fg, v; solveKey)[1] +# Point3f(val.x[2][:,3]) +# end + +# Makie.arrows!(ps, nxs; color=:red, linewidth, lengthscale, arrowsize) +# Makie.arrows!(ps, nys; color=:green, linewidth, lengthscale, arrowsize) +# Makie.arrows!(ps, nzs; color=:blue , linewidth, lengthscale, arrowsize) + +# lines!(ps) +# end + + + +# function plot3d(fg; +# linewidth = 0.025, +# lengthscale=0.15f0, +# arrowsize = Vec3f(0.05, 0.05, 0.1), +# solveKey=:parametric, +# vsyms = ls(fg, r"^x"), +# ) + +# ps = map(enumerate(vsyms)) do (i,v) +# val = getVal(fg, v; solveKey)[1] +# if getVariableType(fg, v) == RotVelPos() +# Point3f(val.x[3][1:3]) +# else +# Point3f(val[1:3]) +# end +# end + +# nxs = map(vsyms) do v +# val = getVal(fg, v; solveKey)[1] +# if getVariableType(fg, v) == RotVelPos() +# Point3f(val.x[1][:,1]) +# else +# Point3f(val.x[2][:,1]) +# end +# end +# nys = map(vsyms) do v +# val = getVal(fg, v; solveKey)[1] +# if getVariableType(fg, v) == RotVelPos() +# Point3f(val.x[1][:,2]) +# else +# Point3f(val.x[2][:,2]) +# end +# end +# nzs = map(vsyms) do v +# val = getVal(fg, v; solveKey)[1] +# if getVariableType(fg, v) == RotVelPos() +# Point3f(val.x[1][:,3]) +# else +# Point3f(val.x[2][:,3]) +# end +# end + +# fig = Makie.arrows(ps, nxs; color=:red, linewidth, lengthscale, arrowsize) +# Makie.arrows!(ps, nys; color=:green, linewidth, lengthscale, arrowsize) +# Makie.arrows!(ps, nzs; color=:blue , linewidth, lengthscale, arrowsize) + +# lines!(ps) +# fig +# end \ No newline at end of file diff --git a/src/services/PlotGraphGeneric.jl b/src/services/PlotGraphGeneric.jl index 873235b..9ee8080 100644 --- a/src/services/PlotGraphGeneric.jl +++ b/src/services/PlotGraphGeneric.jl @@ -1,11 +1,12 @@ -function plotGraph3d( +function plot3d!( dfg::AbstractDFG; tag = :POSE, labels::AbstractVector{Symbol} = sortDFG(ls(dfg; tags=[tag;])), # FIXME, better support for multiple trajectories via tags solveKey = :default, + fig = Figure(), title::AbstractString = string( getSessionLabel(dfg), ", solveKey: ", solveKey, @@ -15,7 +16,11 @@ function plotGraph3d( ), drawTrajectory::Bool=true, drawTrajectoryMarkers::Bool = drawTrajectory, - fig = Figure(), + drawTriads::Bool = false, + linewidth = 0.05, + lengthscale=0.15f0, + arrowsize = Vec3f(0.05, 0.05, 0.1), + aspect = :data, ) # @@ -23,14 +28,10 @@ function plotGraph3d( _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.VelPos3}, solvK) = getPPESuggested(_v, solvK)[4:6] _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) - # )) + ax1 = Axis3(fig[1, 1]; title, aspect) _ppes = _getppepos.(getVariable.(dfg, labels), solveKey) # getPPESuggested.(dfg, labels, solveKey) @@ -44,9 +45,39 @@ function plotGraph3d( 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 + _getPos(::RotVelPos, p::ArrayPartition) = p.x[3] + _getPos(::Pose2, p::ArrayPartition) = [p.x[1]...; 0.0] + _getPos(::Pose3, p::ArrayPartition) = p.x[1] + _getPos(::Position{1}, p::AbstractVector) = [p[1]; 0; 0.0] + _getPos(::Position{2}, p::AbstractVector) = [p[1]; p[2]; 0.0] + _getPos(::Position{N} where N, p::AbstractVector) = p[1:3] + + _getRot(::RotVelPos, p::ArrayPartition) = p.x[1] + _getRot(::Pose3, p::ArrayPartition) = p.x[2] + _getRot(::Pose2, p::ArrayPartition) = [p.x[2][1,1] p.x[2][1,2] 0; p.x[2][2,1] p.x[2][2,2] 0; 0 0 1.0] + _getRot(::Position{N} where N, p::ArrayPartition) = diagm(ones(3)) + + pos = [] + nxs = [] + nys = [] + nzs = [] + if drawTriads + # NOTE using val (not PPE), this will help show when PPEs are out of step with val + for var in getVariable.(dfg, labels) + ps = calcMean(getBelief(var, solveKey)) + push!(pos, Point3f(_getPos(getVariableType(var), ps))) + rot = _getRot(getVariableType(var),ps) + push!(nxs, Point3f(rot[:,1]...)) + push!(nys, Point3f(rot[:,2]...)) + push!(nzs, Point3f(rot[:,3]...)) + Makie.arrows!(ax1, pos, nxs; color=:red, linewidth, lengthscale, arrowsize) + Makie.arrows!(ax1, pos, nys; color=:green, linewidth, lengthscale, arrowsize) + Makie.arrows!(ax1, pos, nzs; color=:blue , linewidth, lengthscale, arrowsize) + end + end + fig -end \ No newline at end of file +end diff --git a/src/services/PlotManifolds.jl b/src/services/PlotManifolds.jl index 469050e..a2b0819 100644 --- a/src/services/PlotManifolds.jl +++ b/src/services/PlotManifolds.jl @@ -4,9 +4,9 @@ function plotPoints( ::typeof(SpecialOrthogonal(3)), ps::AbstractVector{<:AbstractMatrix{<:Real}}; - color = :red + color = :red, + scene = Scene(), ) - scene = Scene() cam3d!(scene) wireframe!(scene, Makie.Sphere( Point3f(0), 1.0), color=:gray) Makie.scale!(scene, 1.0, 1.0, 1.0) diff --git a/src/services/TodoConsolidate.jl b/src/services/TodoConsolidate.jl index 1940e5c..7b5b025 100644 --- a/src/services/TodoConsolidate.jl +++ b/src/services/TodoConsolidate.jl @@ -276,55 +276,7 @@ end ## -function plot3d(fg; - linewidth = 0.025, - lengthscale=0.15f0, - arrowsize = Vec3f(0.05, 0.05, 0.1), - solveKey=:parametric, - vsyms = ls(fg, r"^x"), -) - - ps = map(enumerate(vsyms)) do (i,v) - val = getVal(fg, v; solveKey)[1] - if getVariableType(fg, v) == RotVelPos() - Point3f(val.x[3][1:3]) - else - Point3f(val[1:3]) - end - end - - nxs = map(vsyms) do v - val = getVal(fg, v; solveKey)[1] - if getVariableType(fg, v) == RotVelPos() - Point3f(val.x[1][:,1]) - else - Point3f(val.x[2][:,1]) - end - end - nys = map(vsyms) do v - val = getVal(fg, v; solveKey)[1] - if getVariableType(fg, v) == RotVelPos() - Point3f(val.x[1][:,2]) - else - Point3f(val.x[2][:,2]) - end - end - nzs = map(vsyms) do v - val = getVal(fg, v; solveKey)[1] - if getVariableType(fg, v) == RotVelPos() - Point3f(val.x[1][:,3]) - else - Point3f(val.x[2][:,3]) - end - end - - fig = Makie.arrows(ps, nxs; color=:red, linewidth, lengthscale, arrowsize) - Makie.arrows!(ps, nys; color=:green, linewidth, lengthscale, arrowsize) - Makie.arrows!(ps, nzs; color=:blue , linewidth, lengthscale, arrowsize) - lines!(ps) - fig -end function plot3d_p3(poses::Vector; @@ -355,39 +307,6 @@ function plot3d_p3(poses::Vector; fig end -function plot3d!(fg; - linewidth = 0.025, - lengthscale=0.15f0, - arrowsize = Vec3f(0.05, 0.05, 0.1), - solveKey=:parametric, - vsyms = ls(fg, r"^x"), -) - - - ps = map(enumerate(vsyms)) do (i,v) - val = getVal(fg, v; solveKey)[1] - Point3f(val[1:3]) - end - - nxs = map(vsyms) do v - val = getVal(fg, v; solveKey)[1] - Point3f(val.x[2][:,1]) - end - nys = map(vsyms) do v - val = getVal(fg, v; solveKey)[1] - Point3f(val.x[2][:,2]) - end - nzs = map(vsyms) do v - val = getVal(fg, v; solveKey)[1] - Point3f(val.x[2][:,3]) - end - - Makie.arrows!(ps, nxs; color=:red, linewidth, lengthscale, arrowsize) - Makie.arrows!(ps, nys; color=:green, linewidth, lengthscale, arrowsize) - Makie.arrows!(ps, nzs; color=:blue , linewidth, lengthscale, arrowsize) - - lines!(ps) -end