Merge pull request #960 from JuliaRobotics/23Q2/enh/features

adding image features

Author: dehann

Project.toml

@@ -57,14 +57,14 @@ Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02"
 YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6"
 
 [compat]
-ApproxManifoldProducts = "0.6"
+ApproxManifoldProducts = "0.6, 0.7"
 AprilTags = "0.8, 0.9"
 Colors = "0.12"
 ColorVectorSpace = "0.9"
 Combinatorics = "1"
 CoordinateTransformations = "0.5, 0.6"
 DataStructures = "0.17, 0.18"
-DistributedFactorGraphs = "0.20.1"
+DistributedFactorGraphs = "0.20.1, 0.21"
 Distributions = "0.25"
 DocStringExtensions = "0.8, 0.9"
 FFTW = "1"

src/3rdParty/_PCL/_PCL.jl

@@ -37,6 +37,7 @@ import Base: getindex, setindex!, resize!, cat, convert, sizeof, hasproperty, ge
 
 # gets overloaded
 import Manifolds: apply
+import DistributedFactorGraphs: packBlob, unpackBlob
 import IncrementalInference: ArrayPartition
 
 ## hold off on exports, users can in the mean-time use/import via e.g. _PCL.PointXYZ

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

@@ -483,4 +483,6 @@ Base.show(io::IO, ::MIME"application/prs.juno.inline", pc::PointCloud) = show(io
 
 
 
+
+
 #

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

@@ -4,6 +4,38 @@ export findObjectVariablesFromWorld, previewObjectSubcloudInLocalFromWorld
 export calcAxes3D
 
 
+
+## Special case for LIDAR
+#FIXME convert to type save("pc.las", pc) supports and use pack unpack Blob
+function DistributedFactorGraphs.packBlob(::Type{format"LAS"}, pointCloud::_PCL.PointCloud)
+  mimetype = "application/octet-stream; ext=las"
+  io = IOBuffer()
+  _PCL.saveLAS(Stream{format"LAS"}(io), pointCloud) 
+  blob = take!(io)
+  return blob, mimetype
+end
+
+function DistributedFactorGraphs.unpackBlob(::Type{format"LAS"}, blob::Vector{UInt8})
+  io = IOBuffer(blob)
+  ioStr = Stream{format"LAS"}(io)
+  pc = _PCL.loadLAS(ioStr)
+  return pc
+end
+
+
+function getPointCloud(fg::AbstractDFG, vlbl::Symbol, entry_label::Symbol)
+entry, blob = getData(fg, vlbl, entry_label)
+return unpackBlob(MIME(entry.mimeType), blob)
+end
+
+
+function getPointCloud_prepPoints(fg, vlbl, entry_label; minrange=0.0, maxrange=999.0)
+pc = getPointCloud(fg, vlbl, entry_label)
+return _PCL._prepPointCloud(pc; minrange, maxrange)
+end
+
+
+
 function getDataPointCloud(
   nfg::AbstractDFG,
   varlbl, 

src/ExportAPI.jl

@@ -49,4 +49,5 @@ export
   SASDebug
 
 export calcPointsInPoly
-export inside, AxisAlignedBoundingBox, OrientedBoundingBox
+export inside, AxisAlignedBoundingBox, OrientedBoundingBox
+export packBlob, unpackBlob

src/images/imagedata.jl

@@ -18,17 +18,38 @@ DevNotes
 
 See also: [`makeImage`](@ref)
 """
-function toFormat(format::DataType,
-                  img::AbstractMatrix{<:Colorant} )
+function toFormat(
+  format::Type{format"PNG"},
+  img::AbstractMatrix{<:Colorant}
+)
   #
   io = IOBuffer()
   pngSm = Stream(format, io)
   save(pngSm, img)  # think FileIO is required for this
   take!(io)
 end
 
-toFormat(img::AbstractMatrix{<:Colorant}) = toFormat(format"PNG", img)
-
+@deprecate toFormat(img::AbstractMatrix{<:Colorant}) toFormat(format"PNG", img)
+
+
+## Using default FileIO together with ImageIO 
+# function DistributedFactorGraphs.packBlob(
+#   fmt::Type{format"PNG"}, 
+#   img::Union{<:AbstractMatrix{<:Colorant},<:AbstractMatrix{UInt8}}
+# )
+#   blob = toFormat(fmt, img)
+#   mimetype = "image/png"
+#   return blob, mimetype
+# end
+# function DistributedFactorGraphs.unpackBlob(
+#   ::Type{format"PNG"}, 
+#   blob::Vector{UInt8}
+# )
+#   io = IOBuffer(blob)
+#   ioStr = Stream{format"PNG"}(io)
+#   return load(ioStr)
+#   # ImageMagick.readblob(imgBytes)
+# end
 
 """
     $SIGNATURES

src/images/imagedraw.jl

@@ -5,20 +5,23 @@ using .ImageDraw
 using .Colors
 
 export makeImage!
+export drawKeypointsAndMask
 
 
-function makeImage!(pc::Caesar._PCL.PointCloud,
-                    x_domain::Tuple{<:Real,<:Real}=(-1000,1000),
-                    y_domain::Tuple{<:Real,<:Real}=x_domain;
-                    pose=nothing,
-                    ppose=nothing,
-                    rows::Integer=1000, 
-                    cols::Integer=rows,
-                    color::C=Gray(0.1),
-                    trajCol=Gray(1.0),
-                    img::AbstractMatrix{<:Colorant} = Gray.(zeros(rows,cols)),
-                    circle_size::Real=1,
-                    drawkws... ) where {C <: Colorant}
+function makeImage!(
+  pc::Caesar._PCL.PointCloud,
+  x_domain::Tuple{<:Real,<:Real}=(-1000,1000),
+  y_domain::Tuple{<:Real,<:Real}=x_domain;
+  pose=nothing,
+  ppose=nothing,
+  rows::Integer=1000, 
+  cols::Integer=rows,
+  color::C=Gray(0.1),
+  trajCol=Gray(1.0),
+  img::AbstractMatrix{<:Colorant} = Gray.(zeros(rows,cols)),
+  circle_size::Real=1,
+  drawkws... 
+) where {C <: Colorant}
   #
 
   x_range = (x_domain[2]-x_domain[1])
@@ -49,3 +52,21 @@ function makeImage!(pc::Caesar._PCL.PointCloud,
 
   return img
 end
+
+
+function drawKeypointsAndMask(
+  img::AbstractMatrix,
+  keypoints::AbstractVector{<:CartesianIndex}, 
+  mask::AbstractMatrix;
+  markerColor::AbstractRGB=RGB{N0f8}(1,0,0),
+  markerSize::Int = 20,
+)
+  cimgA_ = applyMaskImage(img, mask)
+  cimgA = convert.(RGB{N0f8}, cimgA_)
+
+  for ret_ in keypoints
+    draw!(cimgA, Cross(Point(ret_.I[2], ret_.I[1]), markerSize), markerColor) 
+  end
+
+  return cimgA
+end

src/images/images.jl

@@ -6,8 +6,12 @@
 
 using ColorVectorSpace
 using .Images
+import .GeoPr
+# import GeometricalPredicates as GeoPr
 
 export applyMaskImage
+export makeMaskImage
+export makeMaskImages
 export imhcatPretty
 
 """
@@ -36,6 +40,71 @@ function toImage(msgd::Dict{String,Any})
   end
 end
 
+function makeMaskImage(
+  img::AbstractMatrix, 
+  nodes::AbstractVector = [
+    (180.0, 0.0);  # ll
+    (10.0, 600.0); # lr
+    (0.0, 600.0);  # ur
+    (0.0, 0.0);    # ul
+  ],
+)
+  h,w = size(img)
+  # h,w = 400,640
+  buffer = zeros(UInt8, h, w)
+  
+  points = []
+  for nd in nodes
+    push!(points, GeoPr.Point(nd...))
+  end
+  # (ll,lr,ur,ul)
+  poly = GeoPr.Polygon(points...)
+  
+  # populate buffer
+  for x in collect(1:1:h), y in collect(1:1:w)
+    if GeoPr.inpolygon(poly, GeoPr.Point(x, y)) 
+      buffer[x, y] = UInt8(1) 
+    end 
+  end
+
+  return buffer, poly
+end
+
+
+"""
+    $SIGNATURES
+
+
+```julia
+jstr_Mask_BOT = \"""
+[
+  [[180.0,0.0],[10.0,600.0],[0.0,600.0],[0.0,0.0]],
+  [[400.0,0.0],[400.0,640.0],[310.0,640.0],[315.0,165.0],[360.0,0.0],[399.0,0.0]]
+]
+\"""
+
+mnodes = JSON3.read(jstr_Mask_BOT)
+mask_u8, polys = makeMaskImages(img, mnodes)
+```
+"""
+function makeMaskImages(
+  img::AbstractMatrix,
+  mnodes::AbstractVector{<:AbstractVector}
+)
+  mask = zeros(UInt8, size(img)...)
+  polys = []
+
+  for nodes in mnodes
+    @show nodes
+    mk, poly = makeMaskImage(img, nodes)
+    mask += mk
+    push!(polys, poly)
+  end
+
+  return mask, polys
+end
+
+
 """
     $SIGNATURES