fixes, wip

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/dev/ScatterSensitivityAnalysis.jl

@@ -18,7 +18,7 @@ Caesar.plotScatterAlign(sap_, bw=0.001)
 ##
 
 # snt_ = deepcopy(snt)
-snt = overlayScatterMutate(sap_; offsetTrans=[-20.0;+100], sample_count=200, bw=1e-4, user_coords=[0;-50;0.]) #, user_coords=snt_.best_coords)
+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)
 
 
@@ -27,10 +27,10 @@ plotScatterAlign(snt)
 ## sensitivity sweep on bw
 
 
-SNT_08 = [[overlayScatterMutate(sap_; offsetTrans=[-20.0;+100], sample_count=150, bw) for i in 1:100] for bw in exp10.(-6:1:-1)];
+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_08")
-@save "/tmp/caesar/rex/SNT_08/SNT_08.jld2" SNT_08
+mkdir("/tmp/caesar/rex/SNT_10")
+@save "/tmp/caesar/rex/SNT_10/SNT_10.jld2" SNT_10
 
 ## histogram sensitivities
 

src/images/ScatterAlignPose2.jl

@@ -133,10 +133,10 @@ Notes:
 - `tf` is a Manifolds.jl type `::ProductRepr` (or newer `::ArrayPartition`) to represent a `SpecialEuclidean(2)` manifold point.
 """
 function overlayScatter(sap::ScatterAlignPose2, 
-                        trans::AbstractVector{<:Real}=SA[0;0.0],
+                        trans::AbstractVector{<:Real}=[0;0.0],
                         rot::Real=0.0;
                         user_coords = [trans; rot],
-                        offsetTrans::AbstractVector{<:Real}=SA[0;0.0],
+                        offsetTrans::AbstractVector{<:Real}=[0;0.0],
                         user_offset = [offsetTrans;0.0],
                         score=Ref(0.0),
                         sample_count::Integer=sap.sample_count,
@@ -158,8 +158,18 @@ function overlayScatter(sap::ScatterAlignPose2,
   # not efficient, but okay for here
   pTq(xyr=user_coords) = exp(M, e0, hat(M, e0, xyr))
 
+  
   R0 = e0.parts[2]
-  _pts2_ = map(pt->ProductRepr(pt, R0), pts2_)
+  _pts2_nt = map(pt->ProductRepr(pt, R0), pts2_)
+  
+  # add user_offset
+  uo = exp(M, e0, hat(M, e0, user_offset))
+  _pts2_ = map(pt->Manifolds.compose(M, uo, pt), _pts2_nt)
+  for (i,pt) in enumerate(_pts2_)
+    pt.parts[2] .= R0
+    # overwrite original for updated return values
+    pts2_[i] .= pt.parts[1]
+  end
 
   # take p as reference identity
   pts2T_ = map(pt->Manifolds.compose(M, pTq(), pt).parts[1], _pts2_)