Merge pull request #752 from JuliaRobotics/21Q4/ex/analyzescatteralign

analysis code for radar scatter map alignments

Author: dehann

examples/marine/asv/rex/RExFeed_Bag.jl

@@ -46,7 +46,7 @@ rostypegen()
 # No using needed because we're specifying by full name.
 # using .sensor_msgs.msg
 # using .seagrant_msgs.msg
- 
+
 ## Load Caesar mmisam stuff
 using Caesar
 using RoME
@@ -246,7 +246,7 @@ function main(;iters::Integer=50)
 
     init_node("rex_feed")
     # find the bagfile
-    bagfile = joinpath(ENV["HOME"],"data","Marine","lidar_radar.bag")
+    bagfile = joinpath(ENV["HOME"],"data","Marine","basin_radar.bag") # lidar_radar.bag
     bagSubscriber = RosbagSubscriber(bagfile)
 
     # Initialization
@@ -296,7 +296,9 @@ end
 ##
 
 
-main(iters=300) # 275
+main(iters=100) 
+
+# 275
 
 
 ## after the graph is saved it can be loaded and the datastores retrieved

examples/marine/asv/rex/ScanMatcher.jl

@@ -9,20 +9,11 @@ using Manifolds
 
 using ImageView
 
-# using GraphPlot
-# using DistributedFactorGraphs
-# using IncrementalInference, RoME
-# using DocStringExtensions
-
-import Rotations as _Rot
-# using CoordinateTransformations
-# using ImageTransformations
-
-# using LinearAlgebra
-# using Optim
+# import Rotations as _Rot
 
 # using Caesar: ScatterAlignPose2
 
+
 ##
 
 # Where to fetch data
@@ -81,70 +72,32 @@ sweeps = map(s -> s/maximum(s), sweeps);
 # newfg = loadDFG("$dfgDataFolder/newfg")
 
 startsweep = 5
-endsweep = 20
+endsweep = 26
 graphinit = true
 
 len = size(sweeps[5],1)
 domain = (range(-100,100,length=976),range(-100,100,length=976))
 
 ## use a standard builder
 
-STEP = 3
+STEP = 20
 sweeps_ = sweeps[startsweep:STEP:endsweep]
 
 # build the graph
 newfg = buildGraphChain!( sweeps_,
                           ScatterAlignPose2,
                           (_, data) -> (data.currData,data.nextData,domain),
-                          stopAfter=5,
+                          stopAfter=1,
                           doRef = false,
                           inflation_fct=0.0,
                           solverParams=SolverParams(graphinit=true, inflateCycles=1) )
 #
 
 
 
-## plotting function for understanding
-
-
-
-
-
-## dev testing on one scatter image with known transform
-
-# sweeps_ = sweeps[5:6]
-# sweeps_[2] =sweeps[5]
-
-# domain = (range(-100,100,length=976),range(-100,100,length=976))
-
-# newfg = buildGraphChain!( sweeps_,
-#                           ScatterAlignPose2,
-#                           (_, data) -> (data.currData,data.nextData,domain),
-#                           stopAfter=1,
-#                           doRef = false,
-#                           inflation_fct=0.0,
-#                           solverParams=SolverParams(graphinit=false, inflateCycles=1) )
-# #
-
-##
-
-# fct = getFactorType(newfg, :x0x1f1)
-# Xtup = sampleFactor(newfg, :x0x1f1)
-# e0 = identity_element(SpecialEuclidean(2))
-# δ = calcFactorResidualTemporary(fct, (Pose2,Pose2), Xtup, (e0, e0))
-
 ##
 
 
-# Run the initialization (very slow right now)
-# initAll!(newfg)
-
-# Factor debugging
-# fs = getFactorFunction.(getFactor.(newfg, lsf(newfg)))
-# fs = filter(f -> f isa ScanMatcherPose2, fs)
-# pf = convert.(PackedAlignRadarPose3, fs)
-# convert.(ScanMatcherPose2, pf)
-
 # Save the graph
 saveDFG("$dfgDataFolder/segment_test.tar.gz", newfg);
 
@@ -158,63 +111,11 @@ pts = getPoints(X1_)
 # solving will internally call initAll!(newfg)
 tree = solveTree!(newfg)
 
-## Looking at the results
-using Plots
-
-ppes = map(v -> getSuggestedPPE(getPPE(getVariable(newfg, v))), ls(newfg))
-x = map(ppe -> ppe[1], ppes); y = map(ppe -> ppe[2], ppes); h = map(ppe -> ppe[3], ppes)
-Plots.plot(x, y, title="Path Plot", lw=3)
-
-
-## Stuff
-using Optim
-
-
-cost(tf, im1, im2) = evaluateTransform(im1,im2, tf )
-
-
-# Plotting
-xrange = -100.0:1.0:100.0
-hrange = -pi:0.1:pi
-val = reshape(
-  [sweepx(sweeps[10],sweeps[11],xrange); sweepx(sweep_original[10],sweep_original[11],xrange)],
-  length(xrange), 2)
-Plots.plot(xrange,val)
-# Heading
-val = reshape(
-  [sweeph(sweeps[10],sweeps[11],hrange); sweeph(sweep_original[10],sweep_original[11],hrange)],
-  length(hrange), 2)
-Plots.plot(hrange,val)
-
-corr_func = (a,b)->sqrt(sum((a .- 0.5).*(b .- 0.5)))
-val = reshape(
-  [sweepx(sweeps[10],sweeps[11],xrange,diff_func=corr_func);
-  sweepx(sweep_original[10],sweep_original[11],xrange,diff_func=corr_func)],
-  length(xrange), 2)
-Plots.plot(xrange,val)
-
-## Sweep plotting
-# sanity check: identity transform should yield zero cost
-# @assert evaluateTransform(sweeps[11],sweeps[11],0.,0.,0.) == 0 "There's error with no transform!"
-
-# let's try small displacements:
-# sweepx(im1, im2, xrange) = (x->@show evaluateTransform(im1,im2,x,0.,0.)).(xrange)
-# sweepy(im1, im2, yrange) = (y->@show evaluateTransform(im1,im2,0.,y,0.)).(yrange)
-# sweeph(im1, im2, hrange) = (h->@show evaluateTransform(im1,im2,0.,0.,h)).(hrange)
 
+##
 
-# using Plots
-# xrange = -10:0.1:10
-# hrange = -pi:0.1:pi
-# Plots.plot(xrange,sweepx(sweeps[10],sweeps[11],xrange))
-# Plots.plot(xrange,sweepy(sweeps[10],sweeps[11],xrange))
-# Plots.plot(hrange,sweeph(sweeps[10],sweeps[11],hrange))
+using Cairo, Gadfly
+Gadfly.set_default_plot_size(40cm,20cm)
 
 
-# fs10 = imfilter(sweeps[10],Kernel.gaussian(3))
-# fs11 = imfilter(sweeps[11],Kernel.gaussian(3))
-# ffs10 = imfilter(fs10,Kernel.gaussian(3))
-# ffs11 = imfilter(fs11,Kernel.gaussian(3))
-#
-# Plots.plot(xrange,sweepx(ffs10,ffs11,xrange))
-# Plots.plot(xrange,sweepy(fs10,fs11,xrange))
+#

examples/marine/asv/rex/dev/ScanMatchAnalysis.jl

@@ -0,0 +1,62 @@
+
+
+## Looking at the results
+using Plots
+
+ppes = map(v -> getPPE(getVariable(newfg, v)).suggested, ls(newfg))
+x = map(ppe -> ppe[1], ppes); y = map(ppe -> ppe[2], ppes); h = map(ppe -> ppe[3], ppes)
+Plots.plot(x, y, title="Path Plot", lw=3)
+
+
+## Stuff
+using Optim
+
+
+cost(tf, im1, im2) = evaluateTransform(im1,im2, tf )
+
+
+# Plotting
+xrange = -100.0:1.0:100.0
+hrange = -pi:0.1:pi
+val = reshape(
+  [sweepx(sweeps[10],sweeps[11],xrange); sweepx(sweep_original[10],sweep_original[11],xrange)],
+  length(xrange), 2)
+Plots.plot(xrange,val)
+# Heading
+val = reshape(
+  [sweeph(sweeps[10],sweeps[11],hrange); sweeph(sweep_original[10],sweep_original[11],hrange)],
+  length(hrange), 2)
+Plots.plot(hrange,val)
+
+corr_func = (a,b)->sqrt(sum((a .- 0.5).*(b .- 0.5)))
+val = reshape(
+  [sweepx(sweeps[10],sweeps[11],xrange,diff_func=corr_func);
+  sweepx(sweep_original[10],sweep_original[11],xrange,diff_func=corr_func)],
+  length(xrange), 2)
+Plots.plot(xrange,val)
+
+## Sweep plotting
+# sanity check: identity transform should yield zero cost
+# @assert evaluateTransform(sweeps[11],sweeps[11],0.,0.,0.) == 0 "There's error with no transform!"
+
+# let's try small displacements:
+# sweepx(im1, im2, xrange) = (x->@show evaluateTransform(im1,im2,x,0.,0.)).(xrange)
+# sweepy(im1, im2, yrange) = (y->@show evaluateTransform(im1,im2,0.,y,0.)).(yrange)
+# sweeph(im1, im2, hrange) = (h->@show evaluateTransform(im1,im2,0.,0.,h)).(hrange)
+
+
+# using Plots
+# xrange = -10:0.1:10
+# hrange = -pi:0.1:pi
+# Plots.plot(xrange,sweepx(sweeps[10],sweeps[11],xrange))
+# Plots.plot(xrange,sweepy(sweeps[10],sweeps[11],xrange))
+# Plots.plot(hrange,sweeph(sweeps[10],sweeps[11],hrange))
+
+
+# fs10 = imfilter(sweeps[10],Kernel.gaussian(3))
+# fs11 = imfilter(sweeps[11],Kernel.gaussian(3))
+# ffs10 = imfilter(fs10,Kernel.gaussian(3))
+# ffs11 = imfilter(fs11,Kernel.gaussian(3))
+#
+# Plots.plot(xrange,sweepx(ffs10,ffs11,xrange))
+# Plots.plot(xrange,sweepy(fs10,fs11,xrange))

examples/marine/asv/rex/dev/ScatterSensitivityAnalysis.jl

@@ -0,0 +1,101 @@
+
+
+using Statistics
+using DataFrames
+using JLD2, FileIO
+
+using Cairo, Gadfly
+Gadfly.set_default_plot_size(40cm,20cm)
+
+## plotting
+
+sap_ = getFactorType(newfg, :x0x1f1)
+
+##
+
+Caesar.plotScatterAlign(sap_, bw=0.001)
+
+##
+
+# snt_ = deepcopy(snt)
+snt = overlayScatterMutate(sap_; user_offset=[-20.0;+100; pi/4], sample_count=200, bw=1e-4, user_coords=[0;-50;0.]) #, user_coords=snt_.best_coords)
+plotScatterAlign(snt)
+
+
+
+
+## sensitivity sweep on bw
+
+
+SNT_10 = [[overlayScatterMutate(sap_; user_offset=[-20.0;+20;pi/4], sample_count=100, bw) for i in 1:200] for bw in exp10.(-6:1:-1)];
+
+mkdir("/tmp/caesar/rex/SNT_10")
+@save "/tmp/caesar/rex/SNT_10/SNT_10.jld2" SNT_10
+
+## histogram sensitivities
+
+df = DataFrame(s=Int[], bw=Float64[], group=Symbol[], μ=Float64[], σ=Float64[], _μ=Float64[], μ_=Float64[] )
+
+for (i,s) in enumerate(-6:-1)
+
+  tl = "e$s"
+  X_ = (x->x.best_coords[1]).(SNT[i])
+  Y_ = (x->x.best_coords[2]).(SNT[i])
+  R_ = (x->x.best_coords[3]).(SNT[i])
+
+  μ_x = Statistics.mean(X_); σ_x = Statistics.std(X_)
+  μ_y = Statistics.mean(Y_); σ_y = Statistics.std(Y_)
+  μ_r = Statistics.mean(R_); σ_r = Statistics.std(R_)
+  
+  _μx, μx_ = μ_x - σ_x, μ_x + σ_x
+  _μy, μy_ = μ_y - σ_y, μ_y + σ_y
+  _μr, μr_ = μ_r - σ_r, μ_r + σ_r
+
+  push!( df, ( s,exp10(s),:x, μ_x,σ_x, _μx,μx_ ) )
+  push!( df, ( s,exp10(s),:y, μ_y,σ_y, _μy,μy_ ) )
+  push!( df, ( s,exp10(s),:r, μ_r,σ_r, _μr,μr_ ) )
+  
+  # Hx = Gadfly.plot(x=X_, Geom.histogram, Guide.title("X, $tl"))
+  # Hy = Gadfly.plot(x=Y_, Geom.histogram, Guide.title("Y, $tl"))
+  # Ht = Gadfly.plot(x=R_, Geom.histogram, Guide.title("θ, $tl"))
+  # hstack(Hx, Hy, Ht);
+end
+
+##
+
+df_xy = filter( row -> row.group != :r, df )
+df_r  = filter( row -> row.group == :r,  df )
+
+p_xy = Gadfly.plot(df_xy, x=:bw, y=:μ, ymin=:_μ, ymax=:μ_, color=:group, Geom.line, Geom.errorbar, Scale.x_log10)
+p_r  = Gadfly.plot(df_r,  x=:bw, y=:μ, ymin=:_μ, ymax=:μ_, color=:group, Geom.line, Geom.errorbar, Scale.x_log10)
+
+hstack(p_xy, p_r)
+
+
+
+##=============================================================================================
+## dev testing on one scatter image with known transform
+
+# sweeps_ = sweeps[5:6]
+# sweeps_[2] = sweeps[5]
+
+# domain = (range(-100,100,length=976),range(-100,100,length=976))
+
+# newfg = buildGraphChain!( sweeps_,
+#                           ScatterAlignPose2,
+#                           (_, data) -> (data.currData,data.nextData,domain),
+#                           stopAfter=1,
+#                           doRef = false,
+#                           inflation_fct=0.0,
+#                           solverParams=SolverParams(graphinit=false, inflateCycles=1) )
+# #
+
+##
+
+# fct = getFactorType(newfg, :x0x1f1)
+# Xtup = sampleFactor(newfg, :x0x1f1)
+# e0 = identity_element(SpecialEuclidean(2))
+# δ = calcFactorResidualTemporary(fct, (Pose2,Pose2), Xtup, (e0, e0))
+
+
+#

examples/marine/rov/highend/lcmserver/pointclouddev.jl

@@ -31,7 +31,7 @@ function lcmpointcloudmsg!(vc, slaml::SLAMWrapper,
   # push to DrakeVisualizer
   #=
   position = Translation(msg[:pos]...)
-  orientation = Rotations.Quat(msg[:orientation]...)
+  orientation = _Rot.UnitQuaternion(msg[:orientation]...)
   Tf = position ∘ LinearMap(orientation)
   vsym = Symbol(vert.label)
   setgeometry!(vc[:submaps][vsym], Triad())

examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl

@@ -8,16 +8,16 @@ import DrakeVisualizer: Triad
 import Base: convert
 using Base: Test
 
-
+import Rotations as _Rot
 
 
 # random testing and converstion to be moved out
 
 function convert{T <: CoordinateTransformations.AffineMap}(::Type{T}, x::SE3)
   q = convert(TransformUtils.Quaternion, x.R)
-  Translation(x.t...) ∘ LinearMap( Rotations.Quat(q.s, q.v...) )
+  Translation(x.t...) ∘ LinearMap( _Rot.UnitQuaternion(q.s, q.v...) )
 end
-function convert{T <: CoordinateTransformations.AffineMap{Rotations.Quat{Float64}}}(::Type{SE3}, x::T)
+function convert{T <: CoordinateTransformations.AffineMap{_Rot.UnitQuaternion{Float64}}}(::Type{SE3}, x::T)
   SE3(x.v[1:3], TransformUtils.Quaternion(x.m.w, [x.m.x,x.m.y,x.m.z]) )
 end