Merge pull request #907 from JuliaRobotics/22Q3/enh/stash_andmore

test solver using data blobs (via stashing)

Author: dehann

.github/workflows/ci.yml

@@ -16,8 +16,7 @@ jobs:
       fail-fast: false
       matrix:
         version:
-          - '1.6'
-          - '1.7'
+          - '1.8'
           - 'nightly'
         os:
           - ubuntu-latest
@@ -66,7 +65,7 @@ jobs:
 
       - uses: julia-actions/setup-julia@v1
         with:
-          version: 1.7
+          version: 1.8
           arch: x64
 
       - uses: actions/cache@v1
@@ -107,7 +106,7 @@ jobs:
       - uses: actions/checkout@v2
       - uses: julia-actions/setup-julia@v1
         with:
-          version: 1.7
+          version: 1.8
       - name: 'Docs on ${{ github.head_ref }}'
         run: |
           export JULIA_PKG_SERVER=""

Project.toml

@@ -2,10 +2,11 @@ name = "Caesar"
 uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics", "ROS"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.13.4"
+version = "0.13.5"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
+Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
 Combinatorics = "861a8166-3701-5b0c-9a16-15d98fcdc6aa"
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
 DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
@@ -59,7 +60,7 @@ FFTW = "1"
 FileIO = "1"
 ImageCore = "0.8, 0.9"
 ImageMagick = "1"
-IncrementalInference = "0.30"
+IncrementalInference = "0.30, 0.31"
 JLD2 = "0.3, 0.4"
 JSON = "0.20, 0.21"
 JSON2 = "0.3, 0.4"
@@ -71,7 +72,7 @@ Optim = "1"
 ProgressMeter = "1"
 Reexport = "1"
 Requires = "1"
-RoME = "0.20"
+RoME = "0.20, 0.21"
 Rotations = "1.1"
 StaticArrays = "1"
 StatsBase = "0.33"

docs/src/examples/custom_factor_features.md

@@ -20,8 +20,8 @@ The MM-iSAMv2 algorithm relies on the Kolmogorov-Criteria as well as uncorrelate
 
 At present `cfo` contains three main fields:
 - `cfo.factor::MyFactor` the factor object as defined in the `struct` definition,
-- `cfo.metadata::FactorMetadata`, which is currently under development and likely to change.
-  - This contains references to the connected variables to the factor and more, and is useful for large data retrieval such as used in Terrain Relative Navigation (TRN).
+- `cfo.fullvariables`, which can be used for large data blob retrieval such as used in Terrain Relative Navigation (TRN).
+  - Also see [Stashing and Caching](@ref section_stash_and_cache)
 - `cfo._sampleIdx` is the index of which computational sample is currently being calculated.
 
 

src/3rdParty/_PCL/services/PlottingUtilsPCL.jl

@@ -7,7 +7,7 @@
 ## _PCL plotting utils
 ## =====================================================================================
 
-# FIXME move pointcloud plotting to Arena.jl instead
+# NOTE moved pointcloud plotting to and see PR Arena.jl#94
 function plotPointCloud(pc::PointCloud)
   x = (s->s.data[1]).(pc.points)
   y = (s->s.data[2]).(pc.points)

src/3rdParty/_PCL/services/PointCloud.jl

@@ -364,7 +364,11 @@ end
 ## =========================================================================================================
 
 
-function _filterMinRange(pts, minrange, maxrange)
+function _filterMinRange(
+  pts::AbstractVector{<:AbstractVector{<:Real}}, 
+  minrange::Real, 
+  maxrange::Real
+)
   # filter pointcloud section of interest
   msk = findall(x-> minrange < norm(x) < maxrange, pts)
   pts[msk]

src/Caesar.jl

@@ -31,6 +31,7 @@ using
   CoordinateTransformations,
   JSON,
   JSON2,
+  Base64,
   FileIO,
   DataStructures,
   ProgressMeter,
@@ -100,6 +101,7 @@ function __init__()
     include("images/ScatterAlignPose2.jl")
 
     @require Gadfly="c91e804a-d5a3-530f-b6f0-dfbca275c004" include("plotting/ScatterAlignPlotting.jl")
+    @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("images/ROSConversions.jl")
   end
   @require Distributed="8ba89e20-285c-5b6f-9357-94700520ee1b" include("images/DistributedUtils.jl")
 end

src/beamforming/SASBearing2D.jl

@@ -120,14 +120,14 @@ function (cfo::CalcFactor{<:SASBearing2D})( meas,
                                             XX...  )
   #
   sas2d = cfo.factor
-  userdata = cfo.metadata
+  # userdata = cfo.metadata # OUT OF DATE, use CalcFactor.fullvariables
   idx = cfo._sampleIdx
   #
   dx, dy, azi = 0.0, 0.0, 0.0
   thread_data = sas2d.threadreuse[Threads.threadid()]
   res = [0.0;]
 
-  if string(userdata.solvefor)[1] == 'l'
+  if cf.solvefor == 1
     # Solving all poses to Landmark
     # Reference the filter positions locally (first element)
 
@@ -140,8 +140,8 @@ function (cfo::CalcFactor{<:SASBearing2D})( meas,
       # ccall(:jl_, Void, (Any,), "idx=$(idx)\n")
       thread_data.oncebackidx[1] = idx
       #run for eacn new idx
-      thread_data.workvarlist = userdata.variablelist[2:end] # skip landmark element
-      thread_data.looelement = findfirst(thread_data.workvarlist .== userdata.solvefor)
+      thread_data.workvarlist = cfo.variablelist[2:end] # skip landmark element
+      thread_data.looelement = findfirst(thread_data.workvarlist .== cfo.solvefor)
       thread_data.elementset = setdiff(Int[1:length(thread_data.workvarlist);],   [thread_data.looelement;])
 
       # Reference first or second element

src/images/ROSConversions.jl

@@ -0,0 +1,61 @@
+@info "Caesar.jl is loading tools relating to Images.jl and RobotOS.jl"
+
+using .RobotOS
+
+import Unmarshal: unmarshal
+
+@rosimport std_msgs.msg: Header
+@rosimport sensor_msgs.msg: Image
+
+
+function unmarshal(
+  header::Main.std_msgs.msg.Header
+)
+  #
+  Dict{String,Any}( 
+    "seq" => header.seq,
+    "stamp" => Dict{String,Int}(
+      "secs"=>header.stamp.secs, 
+      "nsecs"=>header.stamp.nsecs
+    ),
+    "frame_id" => header.frame_id,
+    "_type" => "ROS1/std_msgs/Header" 
+  )
+end
+
+function unmarshal(
+  msg::Main.sensor_msgs.msg.Image
+)
+  Dict{String,Any}(
+    "width"  => Int(msg.width),
+    "height" => Int(msg.height),
+    "step" => Int(msg.step),
+    "data_b64" => base64encode(msg.data),
+    "encoding" => msg.encoding,
+    "is_bigendian" => msg.is_bigendian === 0x01,
+    "header" => unmarshal(msg.header),
+    "_type" => "ROS1/sensor_msgs/Image?base64",
+    "description" => "Caesar.toImage(JSON.parse(jstr))"
+  )
+end
+
+
+function toImage(msgd::Dict{String,Any})
+  data = base64decode(msgd["data_b64"])
+  h, w = msgd["height"], msgd["width"]
+
+  if msgd["encoding"] == "mono8"
+    img = Matrix{Gray{N0f8}}(undef, h, w)
+    # assuming one endian type for now, TODO both little and big endian
+    for i in 1:h, j in 1:w
+      img[i,j] = Gray{N0f8}(data[msgd["step"]*(i-1)+j]/255)
+    end
+    img
+  else
+    error("Conversion for ROS sensor_msgs.Image encoding not implemented yet $(msgd["encoding"])")
+  end
+end
+
+toImage(msg::Main.sensor_msgs.msg.Image) = unmarshal(msg) |> toImage
+
+#

src/images/ScatterAlignPose2.jl

@@ -1,14 +1,15 @@
 
 
-import IncrementalInference: getSample, preambleCache
+import IncrementalInference: getSample, preambleCache, _update!
 import Base: convert, show
 # import DistributedFactorGraphs: getManifold
-import ApproxManifoldProducts: sample
+import ApproxManifoldProducts: sample, _update!
 
+using UUIDs
 using .Images
 
 export ScatterAlignPose2, PackedScatterAlignPose2
-export ScatterAlignPose3
+export ScatterAlignPose3, PackedScatterAlignPose3
 
 export overlayScatter, overlayScatterMutate
 
@@ -67,7 +68,7 @@ Base.@kwdef struct ScatterAlign{P,
   Arg 0 < `rescale` ≤ 1 is also used to rescale the images to lower resolution for speed. """
   gridscale::Float64 = 1.0
   """ how many heatmap sampled particles to use for mmd alignment """
-  sample_count::Int  = 100
+  sample_count::Int  = 500
   """ bandwidth to use for mmd """
   bw::Float64        = 1.0
   """ EXPERIMENTAL, flag whether to use 'stashing' for large point cloud, see [Stash & Cache](@ref section_stash_unstash) """
@@ -196,7 +197,11 @@ getManifold(::IIF.InstanceType{<:ScatterAlignPose2}) = getManifold(Pose2Pose2)
 getManifold(::IIF.InstanceType{<:ScatterAlignPose3}) = getManifold(Pose3Pose3)
 
 # runs once upon addFactor! and returns object later used as `cache`
-function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, fnc::Union{<:ScatterAlignPose2,<:ScatterAlignPose3})
+function preambleCache(
+  dfg::AbstractDFG, 
+  vars::AbstractVector{<:DFGVariable}, 
+  fnc::Union{<:ScatterAlignPose2,<:ScatterAlignPose3}
+)
   #
   M = getManifold(_getPoseType(fnc.align))
   e0 = getPointIdentity(M)
@@ -205,10 +210,14 @@ function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, fn
   for (va,de,cl) in zip(vars,[fnc.align.dataEntry_cloud1,fnc.align.dataEntry_cloud2],[fnc.align.cloud1,fnc.align.cloud2])
     if fnc.align.useStashing 
       @assert 0 < length(de) "cannot reconstitute ScatterAlignPose2 without necessary data entry, only have $de"
-      _, db = getData(dfg, getLabel(va), Symbol(de)) # fnc.align.dataStoreHint
+      _, db = getData(dfg, getLabel(va), UUID(de)) # fnc.align.dataStoreHint
+      # Assume PackedManifoldKernelDensity
       cld = convert(SamplableBelief, String(take!(IOBuffer(db))))
-      # cld = unpackDistribution(strdstr)
-      IIF._update!(cl, cld)
+      # payload = JSON.parse(String(take!(IOBuffer(db))))
+      # dstr = unmarshal PackedManifoldKernelDensity
+      # cld = unpackDistribution(dstr)
+      # update either a HGD or MKD
+      _update!(cl, cld)
     end
   end
 
@@ -409,24 +418,42 @@ Base.@kwdef struct PackedScatterAlignPose2 <: AbstractPackedFactor
   dataStoreHint::String = ""
 end
 
+Base.@kwdef struct PackedScatterAlignPose3 <: AbstractPackedFactor
+  _type::String = "Caesar.PackedScatterAlignPose3"
+  cloud1::_PARCHABLE_PACKED_CLOUD
+  cloud2::_PARCHABLE_PACKED_CLOUD
+  gridscale::Float64 = 1.0
+  sample_count::Int = 50
+  bw::Float64 = 0.01
+  """ EXPERIMENTAL, flag whether to use 'stashing' for large point cloud, see [Stash & Cache](@ref section_stash_unstash) """
+  useStashing::Bool = false
+  """ DataEntry ID for stash store of cloud 1 & 2 """
+  dataEntry_cloud1::String = ""
+  dataEntry_cloud2::String = ""
+  """ Data store hint where likely to find the data entries and blobs for reconstructing cloud1 and cloud2"""
+  dataStoreHint::String = ""
+end
 
-function convert(::Type{<:PackedScatterAlignPose2}, arp::ScatterAlignPose2)
-
+function convert(
+  ::Type{T}, 
+  arp::Union{<:ScatterAlignPose2,<:ScatterAlignPose3}
+) where {T <: Union{<:PackedScatterAlignPose2,<:PackedScatterAlignPose3}}
+  #
   cld1 = arp.align.cloud1
   cld2 = arp.align.cloud2
 
   # reconstitute full type during the preambleCache step
   if arp.align.useStashing
-    @assert length(arp.align.dataEntry_cloud1) !== 0 "packing of ScatterAlignPose2 asked to be `useStashing=true`` yet no `.dataEntry_cloud1` exists for later loading"
+    @assert length(arp.align.dataEntry_cloud1) !== 0 "packing of ScatterAlignPose asked to be `useStashing=true`` yet no `.dataEntry_cloud1` exists for later loading"
     cld1 = IIF.parchDistribution(arp.align.cloud1)
-    @assert length(arp.align.dataEntry_cloud2) !== 0 "packing of ScatterAlignPose2 asked to be `useStashing=true`` yet no `.dataEntry_cloud2` exists for later loading"
+    @assert length(arp.align.dataEntry_cloud2) !== 0 "packing of ScatterAlignPose asked to be `useStashing=true`` yet no `.dataEntry_cloud2` exists for later loading"
     cld2 = IIF.parchDistribution(arp.align.cloud2)
   end
 
   cloud1 = packDistribution(cld1)
   cloud2 = packDistribution(cld2)
 
-  PackedScatterAlignPose2(;
+  T(;
     cloud1,
     cloud2,
     gridscale = arp.align.gridscale,
@@ -435,22 +462,29 @@ function convert(::Type{<:PackedScatterAlignPose2}, arp::ScatterAlignPose2)
     useStashing = arp.align.useStashing,
     dataEntry_cloud1 = arp.align.dataEntry_cloud1,
     dataEntry_cloud2 = arp.align.dataEntry_cloud2,
-    dataStoreHint = arp.align.dataStoreHint )
+    dataStoreHint = arp.align.dataStoreHint 
+  )
 end
 
-function convert(::Type{<:ScatterAlignPose2}, parp::PackedScatterAlignPose2)
+function convert(
+  ::Type{T}, 
+  parp::Union{<:PackedScatterAlignPose2,<:PackedScatterAlignPose3}
+) where {T <: Union{<:ScatterAlignPose2,<:ScatterAlignPose3}}
   #
-  
+  _selectPoseT(::Type{<:ScatterAlignPose2}) = Pose2
+  _selectPoseT(::Type{<:ScatterAlignPose3}) = Pose3
+
   function _resizeCloudData!(cl::PackedHeatmapGridDensity)
-    typ, col, row = typeof(cl.data[1][1]), Int(cl.data[1][2]), Int(cl.data[2][1])
+    row=Int(cl.data[2][1])
+    typ, col = typeof(cl.data[1][1]), Int(cl.data[1][2])
     resize!(cl.data,row)
     for i in 1:row
       cl.data[i] = Vector{typ}(undef, col)
     end
     nothing
   end
-  _resizeCloudData!(cl::ManifoldKernelDensity) = nothing
-  
+  _resizeCloudData!(cl::PackedManifoldKernelDensity) = nothing
+
   # prep cloud1.data fields for larger data
   if parp.useStashing
     _resizeCloudData!(parp.cloud1)
@@ -461,7 +495,8 @@ function convert(::Type{<:ScatterAlignPose2}, parp::PackedScatterAlignPose2)
   cloud2 = unpackDistribution(parp.cloud2)
 
   # and build the final object
-  ScatterAlignPose2(ScatterAlign{Pose2, typeof(cloud1), typeof(cloud2)}(;
+  poseT = _selectPoseT(T)
+  T(ScatterAlign{poseT, typeof(cloud1), typeof(cloud2)}(;
     cloud1,
     cloud2,
     gridscale=parp.gridscale,

test/ICRA2022_tests/nongaussian_mixture.jl

@@ -18,6 +18,7 @@ using IncrementalInference
 
 # Start with an empty factor graph
 fg = initfg()
+getSolverParams(fg).useMsgLikelihoods = true
 
 # add the first node
 addVariable!(fg, :x0, ContinuousScalar)
@@ -92,6 +93,8 @@ tree = solveGraph!(fg);
 
 ppes = DFG.getPPESuggested.(fg, [:x0;:x1;:x2;:x3])
 
+#
+
 @test isapprox( getPPESuggested(fg, :x0)[1], 0; atol=1)
 @test isapprox( getPPESuggested(fg, :x1)[1], 10; atol=1)
 @test isapprox( getPPESuggested(fg, :x2)[1], 20; atol=2)