Fix plotting for ICRA Tutorial 3

NEWS.md

@@ -0,0 +1,10 @@
+
+
+
+## Changes in v0.9
+
+- Standardize to `plotBelief` instead of previous `plotKDE`, see JuliaRobotics/Caesar.jl#811
+- Major refactor of code placement in src files -- ongoing work through multiple versions.
+
+
+(2022Q1): Starting to individually list larger change in the NEWS.md file.

Project.toml

@@ -2,7 +2,7 @@ name = "RoMEPlotting"
 uuid = "238d586b-a4bf-555c-9891-eda6fc5e55a2"
 keywords = ["mm-iSAM", "SLAM", "plotting", "2D", "robotics", "navigation"]
 desc = "Visualization tools for the RoME.jl package."
-version = "0.8.2"
+version = "0.9.0"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -34,15 +34,15 @@ DistributedFactorGraphs = "0.17, 0.18"
 DocStringExtensions = "0.7, 0.8"
 Fontconfig = "0.3, 0.4"
 Gadfly = "1.3.1"
-IncrementalInference = "0.25, 0.26, 0.27"
+IncrementalInference = "0.26, 0.27"
 KernelDensityEstimate = "0.5"
 KernelDensityEstimatePlotting = "0.1.6"
 Reexport = "0.2, 1"
 Requires = "1"
-RoME = "0.16, 0.17, 0.18"
+RoME = "0.17, 0.18"
 StatsBase = "0.32, 0.33"
 TensorCast = "0.33, 0.4"
-julia = "1.5"
+julia = "1.6"
 
 [extras]
 Flux = "587475ba-b771-5e3f-ad9e-33799f191a9c"

src/CurrentlyUnmaintained.jl

@@ -2,6 +2,94 @@
 
 
 
+function plotLsrScanFeats(br::Array{Float64,2})
+  Cart = zeros(size(br))
+  Cart[:,1] = br[:,2].*cos(br[:,1])
+  Cart[:,2] = br[:,2].*sin(br[:,1])
+  plot(x=Cart[:,1],y=Cart[:,2],Geom.point,
+  Guide.xticks(ticks=collect(-60:10:60)),
+  Guide.yticks(ticks=collect(0:10:80)))
+end
+
+function plotFeatTrackers(trkrs::Dict{Int64,Feature}, bfts::Array{Float64,2})
+  musX = Float64[]
+  varX = Float64[]
+  musY = Float64[]
+  varY = Float64[]
+  allPtsX = Float64[]
+  allPtsY = Float64[]
+
+  for ftr in trkrs
+    pts = getPoints(ftr[2].bel)
+    allPtsX = [allPtsX; vec(pts[1,:])]
+    allPtsY = [allPtsY; vec(pts[2,:])]
+
+    push!(musX, Statistics.mean(vec(pts[1,:])))
+    push!(varX, Statistics.std(vec(pts[1,:])))
+    push!(musY, Statistics.mean(vec(pts[2,:])))
+    push!(varY, Statistics.std(vec(pts[2,:])))
+  end
+
+  X = Float64[]
+  Y = Float64[]
+
+  if size(bfts,2) > 0
+    if bfts[1,1] != 0.0 && bfts[2,1] != 0.0 && bfts[3,1] != 0.0
+      for i in 1:size(bfts,2)
+          u, R = p2c(vec(bfts[:,i]))
+          push!(X, u[1])
+          push!(Y, u[2])
+      end
+    end
+  end
+
+  # Guide.yticks(ticks=collect(-60:10:60)),
+  # Guide.xticks(ticks=collect(0:10:80))
+  p = plot(layer(x=musX, y=musY, Geom.point, Theme(default_color=colorant"red")),
+  layer(x=allPtsX, y=allPtsY, Geom.histogram2d),
+  Guide.yticks(ticks=collect(-70:10:70)),
+  Guide.xticks(ticks=collect(-40:10:80)))
+  for i in 1:length(X)
+    push!(p.layers, Gadfly.layer(x=[0.0;X[i]], y=[0.0;Y[i]], Geom.line, Gadfly.Theme(default_color=colorant"magenta"))[1])
+  end
+  p
+end
+
+
+function saveImgSeq(d::Dict{Int64,Array{Float64,2}}; from::Int=1,to::Int=10,step::Int=1)
+  for i in from:step:to
+    p = plotLsrScanFeats(lsrBR(d[i]));
+    Gadfly.draw(PNG(string("imgs/img",i,".png"),25cm,25cm),p)
+  end
+  nothing
+end
+
+
+
+"""
+    $SIGNATURES
+
+Plot trajectory of Array{,2} with rows as consecutive entries and columns as x,y,theta.
+"""
+function plotTrajectoryArrayPose2(arr::AbstractMatrix__{<:Real};
+                                  spscale::Real=0.5,
+                                  triadStride::Int=50)
+  #
+  @assert size(arr,2)==3
+  trajPlt = Gadfly.plot(x=arr[:,1], y=arr[:,2], Geom.path, Coord.cartesian(fixed=true, aspect_ratio=1))
+
+  if triadStride != -1
+    Xpp = arr[1:triadStride:end,1]
+    Ypp = arr[1:triadStride:end,2]
+    Thpp = arr[1:triadStride:end,3]
+    addXYLineLayers!(trajPlt, Xpp, Ypp, Thpp, l=spscale)
+  end
+  return trajPlt
+end
+
+
+
+
 
 # Victoria Park Plotting functions