wip, standardize old ScalarField test code

Author: dehann

Project.toml

@@ -55,7 +55,8 @@ julia = "1.4"
 
 [extras]
 Flux = "587475ba-b771-5e3f-ad9e-33799f191a9c"
+ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "Flux"]
+test = ["Flux", "ImageCore", "Test"]

data/CanyonDEM.png

@@ -10,6 +10,7 @@ using Reexport
 
 using
   Dates,
+  FileIO,
   Distributed,
   LinearAlgebra,
   Statistics,
@@ -294,6 +295,8 @@ include("canonical/GenerateHelix.jl")
 include("AdditionalUtils.jl")
 include("g2oParser.jl")
 
+# ScalarFields
+include("services/ScalarFields.jl")
 
 # things on their way out
 include("Deprecated.jl")
@@ -304,10 +307,19 @@ using Requires
 function __init__()
   # combining neural networks natively into the non-Gaussian  factor graph object
   @require Flux="587475ba-b771-5e3f-ad9e-33799f191a9c" begin
-    @info "RoME is adding Flux related functionality."
+    @info "Loading RoME.jl tools related to Flux.jl."
     include("factors/flux/models/Pose2OdoNN_01.jl") # until a better way is found to deserialize
     include("factors/flux/MixtureFluxPose2Pose2.jl")
   end
+
+  # Scalar field specifics
+  
+  @require ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534" include("services/RequiresImages.jl")
+  # Images="916415d5-f1e6-5110-898d-aaa5f9f070e0" 
+
+  # @require Interpolations="a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" begin 
+  #   include("services/ScalarFieldsInterpolations.jl")
+  # end
 end
 
 # manifold conversions required during transformation

src/RoME.jl

@@ -0,0 +1,42 @@
+# ScalarField functions related to Images.jl
+
+@info "Loading RoME.jl tools related to ImageCore.jl"
+
+using .ImageCore
+
+export generateField_CanyonDEM
+
+
+"""
+    $SIGNATURES
+
+Loads a sample DEM (as if simulated) on a regular grid... It's the Grand Canyon, 18x18km, 17m
+"""
+function generateField_CanyonDEM( scale=1, N=100; 
+                                  x_is_north=true,
+                                  x_min::Real=-9000, x_max::Real=9000,
+                                  y_min::Real=-9000, y_max::Real=9000)
+    #
+  img_ = load(dirname(dirname(@__DIR__)), "data","CanyonDEM.png") .|> Gray
+  img_ = scale.*Float64.(img_)
+  
+  N_ = minimum([N; size(img_)...])
+  img = img_[1:N_, 1:N_]
+  
+  # flip image so x-axis in plot is North and y-axis is West (ie img[-north,west], because top left is 0,0)
+  _img_ = if x_is_north
+      _img = collect(img')
+      reverse(_img, dims=2)
+  else
+      img
+  end
+  
+  x = range(x_min, x_max, length = size(_img_,1)) # North
+  y = range(y_min, y_max, length = size(_img_,2)) # East
+  
+  return (x, y, _img_)
+end
+
+
+
+#

src/services/RequiresImages.jl

@@ -0,0 +1,69 @@
+# ScalarField related functions loaded when Interpolations.jl is available.
+
+
+"""
+    $SIGNATURES
+
+Load gridded elevation data `dem` into a factor graph `fg` as a collection
+of Point3 variables. Each variable is connected to its 4-neighborhood by
+relative Point3Point3 MvNormal constraints with mean defined by their
+relative position on the grid (`x`, `y`) and covariance `meshEdgeSigma`.
+"""
+function _buildGraphScalarField!( fg::AbstractDFG,
+                                  dem::AbstractMatrix, # assume grayscale image for now
+                                  x::AbstractVector,
+                                  y::AbstractVector;
+                                  solvable::Int=0,
+                                  marginalized::Bool=true,
+                                  meshEdgeSigma=diagm([1;1;1]),
+                                  refKey::Symbol = :simulated )
+  #
+  # assume regular grid
+  dx, dy = x[2]-x[1], y[2]-y[1]
+  for i in 1:length(x)
+    for j in 1:length(y)
+      s = Symbol("pt$(i)_$(j)") # unique identifier
+      pt = addVariable!(fg, s, Point3, solvable=solvable)    
+      setMarginalized!(pt, marginalized) # assume solveKey=:default
+      
+      # ...
+      refVal = [x[i];y[j];dem[i,j]]
+      simPPE = DFG.MeanMaxPPE(refKey, refVal, refVal, refVal)
+      setPPE!(pt, refKey, typeof(simPPE), simPPE)
+      
+      # Regular grid triangulation:
+      #  add factor to (i-1,j)   |
+      #  add factor to (i, j-1)  -
+      #  add factor to (i-1, j-1)  \
+
+      # no edges to prev row on first row
+      if i>1
+        dVal1 = dem[i,j]-dem[i-1,j]
+        f = Point3Point3(MvNormal([dx, 0, dVal1], meshEdgeSigma))
+        addFactor!(fg, [Symbol("pt$(i-1)_$(j)"), s], f, solvable=solvable, graphinit=false)
+      end
+      
+      # no edges to prev column on first column
+      if j>1
+        dVal2 = dem[i,j]-dem[i,j-1]
+        f = Point3Point3(MvNormal([0, dy, dVal2], meshEdgeSigma))
+        addFactor!(fg, [Symbol("pt$(i)_$(j-1)"),s], f, solvable=solvable, graphinit=false)
+      end
+
+      # no edges to add on first element
+      if i>1 && j>1
+        dVal3 = dem[i,j]-dem[i-1,j-1]
+        f = Point3Point3(MvNormal([dx, dy, dVal3], meshEdgeSigma))
+        addFactor!(fg,[Symbol("pt$(i-1)_$(j-1)"),s], f, solvable=solvable, graphinit=false)
+      end
+      
+    end
+  end
+
+  nothing
+end
+
+
+
+
+#

src/services/ScalarFields.jl

@@ -15,6 +15,9 @@ using Statistics
 @error("add test for generateCanonicalFG_Beehive, norm( simulated - default ) < tol")
 
 testfiles = [
+  # new tests wip
+  "testScalarFields.jl"
+
   # any wip fail-fast dev testing
   "testPartialPose3.jl";
 

test/runtests.jl

@@ -0,0 +1,33 @@
+# test scalar field
+
+using Test
+using ImageCore
+using RoME
+
+##
+
+@testset "Basic low-res ScalarField localization" begin
+##
+
+# # load dem (18x18km span, ~17m/px)
+x_min, x_max = -9000, 9000
+y_min, y_max = -9000, 9000
+# north is regular map image up
+x, y, img_ = RoME.generateField_CanyonDEM(1, 100, x_is_north=false, x_min=x_min, x_max=x_max, y_min=y_min, y_max=y_max)
+# flip so north is down along with Images.jl [i,j] --> (x,y)
+img = reverse(img_, dims=1)
+
+
+
+
+
+
+
+##
+end
+
+
+
+
+
+##