CaesarImagesExt.jl

JuliaRobotics · Jul 15, 2023 · ee2cb0e · ee2cb0e
1 parent 0cf404a
commit ee2cb0e
Show file tree

Hide file tree

Showing 10 changed files with 263 additions and 212 deletions.
diff --git a/Project.toml b/Project.toml
@@ -59,12 +59,14 @@ YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6"
 
 [weakdeps]
 AprilTags = "f0fec3d5-a81e-5a6a-8c28-d2b34f3659de"
+Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0"
 ImageFeatures = "92ff4b2b-8094-53d3-b29d-97f740f06cef"
 ImageMagick = "6218d12a-5da1-5696-b52f-db25d2ecc6d1"
 ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [extensions]
 CaesarAprilTagsExt = "AprilTags"
+CaesarImagesExt = "Images"
 CaesarImageFeaturesExt = "ImageFeatures"
 CaesarImageMagickExt = "ImageMagick"
 CaesarZMQExt = "ZMQ"
@@ -118,7 +120,6 @@ BSON = "fbb218c0-5317-5bc6-957e-2ee96dd4b1f0"
 DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab"
 Downloads = "f43a241f-c20a-4ad4-852c-f6b1247861c6"
 GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231"
-Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0"
 PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 RoMEPlotting = "238d586b-a4bf-555c-9891-eda6fc5e55a2"

diff --git a/src/images/images.jl → ext/CaesarImagesExt.jl b/src/images/images.jl → ext/CaesarImagesExt.jl
@@ -1,18 +1,38 @@
+module CaesarImagesExt
 
 @info "Loading Caesar tools related to Images.jl."
 
-# using Images
-# using ImageTransformations
 
+using Images
+# using ImageTransformations
 using ColorVectorSpace
-using .Images
-import .GeoPr
-# import GeometricalPredicates as GeoPr
+using UUIDs
+using TensorCast
+using StaticArrays
+using Manifolds
+using DocStringExtensions
+using ProgressMeter
+using Optim
+
+using Caesar # TODO try reduce to just import Caesar ... below
+import Caesar._PCL as _PCL
+
+# import DistributedFactorGraphs: getManifold
+
+import Base: convert, show
+
+import GeometricalPredicates as GeoPr
+
+import ApproxManifoldProducts: sample, _update!
+import IncrementalInference: getSample, preambleCache, _update!, getManifold
 
-export applyMaskImage
-export makeMaskImage
-export makeMaskImages
-export imhcatPretty
+import Caesar: applyMaskImage, makeMaskImage, makeMaskImages, imhcatPretty, toImage
+import Caesar: writevideo, csmAnimationJoinImgs, csmAnimateSideBySide, makeVideoFromData
+import Caesar: overlayScanMatcher
+import Caesar: ScanMatcherPose2, PackedScanMatcherPose2
+import Caesar: _PARCHABLE_PACKED_CLOUD
+import Caesar: ScatterAlign, ScatterAlignPose2, ScatterAlignPose3
+import Caesar: PackedScatterAlignPose2, PackedScatterAlignPose3
 
 """
     $SIGNATURES
@@ -140,6 +160,10 @@ function imhcatPretty(iml::AbstractMatrix{<:Colorant},
   hcat(iml,imll)
 end
 
+include("Images/ImageToVideoUtils.jl")
+include("Images/ScanMatcherUtils.jl")
+include("Images/ScanMatcherPose2.jl")
+include("Images/ScatterAlignPose2.jl")
 
 
-#
+end # module
diff --git a/src/images/ImageToVideoUtils.jl → ext/Images/ImageToVideoUtils.jl b/src/images/ImageToVideoUtils.jl → ext/Images/ImageToVideoUtils.jl
@@ -1,6 +1,4 @@
 
-export writevideo, csmAnimationJoinImgs, csmAnimateSideBySide
-export makeVideoFromData
 
 
 """

diff --git a/src/images/ScanMatcherPose2.jl → ext/Images/ScanMatcherPose2.jl b/src/images/ScanMatcherPose2.jl → ext/Images/ScanMatcherPose2.jl
@@ -1,52 +1,6 @@
 
 
-import IncrementalInference: getSample
-import Base: convert, show
-# import DistributedFactorGraphs: getManifold
 
-using .Images
-
-export ScanMatcherPose2, PackedScanMatcherPose2
-
-"""
-$TYPEDEF
-
-This is but one incarnation for how radar alignment factor could work, treat it as a starting point.
-
-Notes
-- Stanard `cvt` argument is lambda function to convert incoming images to user convention of image axes,
-  - default `cvt` flips image rows so that Pose2 xy-axes corresponds to img[x,y] -- i.e. rows down and across from top left corner.
-- Use rescale to resize the incoming images for lower resolution (faster) correlations
-- Both images passed to the construct must have the same type some matrix of type `T`.
-
-Example
--------
-```julia
-arp2 = ScanMatcherPose2(img1, img2, 2) # e.g. 2 meters/pixel 
-```
-
-See also: [`overlayScanMatcher`](@ref)
-"""
-struct ScanMatcherPose2{T} <: IIF.AbstractManifoldMinimize
-  """ reference image for scan matching. """
-  im1::Matrix{T}
-  """ test image to scan match against the reference image. """
-  im2::Matrix{T}
-  """ Common grid scale for both images -- i.e. units/pixel.  
-  Constructor uses two arguments `gridlength`*`rescale=1`=`gridscale`.
-  Arg 0 < `rescale` ≤ 1 is also used to rescale the images to lower resolution for speed. """
-  gridscale::Float64
-
-  # replace inner constructor with transform on image
-  ScanMatcherPose2{T}( im1::AbstractMatrix{T}, 
-                      im2::AbstractMatrix{T},
-                      gridlength::Real,
-                      rescale::Real=1,
-                      cvt = (im)->reverse(Images.imresize(im,trunc.(Int, rescale.*size(im))),dims=1)
-                    ) where {T} = new{T}( cvt(im1), 
-                                          cvt(im2),
-                                          rescale*gridlength )
-end
 
 ScanMatcherPose2(im1::AbstractMatrix{T}, im2::AbstractMatrix{T}, w...) where T = ScanMatcherPose2{T}(im1,im2, w...)
 
@@ -93,7 +47,7 @@ Notes:
 - `tf` is a Manifolds.jl type `::ProductRepr` (or newer `::ArrayPartition`) to represent a `SpecialEuclidean(2)` manifold point.
 """
 function overlayScanMatcher(sm::ScanMatcherPose2, 
-                            trans::AbstractVector{<:Real}=Float64[0;0],
+                            trans::Vector{Float64}=Float64[0;0],
                             rot::Real=0.0;
                             score=Ref(0.0),
                             showscore::Bool=true  )
@@ -119,7 +73,7 @@ end
 #
 
 function overlayScanMatcher(sm::ScanMatcherPose2, 
-                            tf = Manifolds.identity_element(SpecialEuclidean(2));
+                            tf::ArrayPartition = Manifolds.identity_element(SpecialEuclidean(2));
                             kw... )
   #
   M = SpecialOrthogonal(2)
@@ -134,11 +88,6 @@ end
 ## Factor serialization below
 ## =========================================================================================
 
-struct PackedScanMatcherPose2 <: AbstractPackedFactor
-  im1::Vector{Vector{Float64}}
-  im2::Vector{Vector{Float64}}
-  gridscale::Float64
-end
 
 function convert(::Type{<:PackedScanMatcherPose2}, arp2::ScanMatcherPose2)
   TensorCast.@cast pim1[row][col] := arp2.im1[row,col]

diff --git a/src/images/ScanMatcherUtils.jl → ext/Images/ScanMatcherUtils.jl b/src/images/ScanMatcherUtils.jl → ext/Images/ScanMatcherUtils.jl
@@ -1,6 +1,5 @@
 # Utilities for scan matching
 
-export overlayScanMatcher
 
 
 # this function uses the sum of squared differences between the two images.

diff --git a/src/images/ScatterAlignPose2.jl → ext/Images/ScatterAlignPose2.jl b/src/images/ScatterAlignPose2.jl → ext/Images/ScatterAlignPose2.jl
@@ -1,113 +1,6 @@
 
 
-import IncrementalInference: getSample, preambleCache, _update!
-import Base: convert, show
-# import DistributedFactorGraphs: getManifold
-import ApproxManifoldProducts: sample, _update!
 
-using UUIDs
-using .Images
-
-export ScatterAlignPose2, PackedScatterAlignPose2
-export ScatterAlignPose3, PackedScatterAlignPose3
-
-export overlayScatter, overlayScatterMutate
-
-"""
-    ScatterAlign{P,H1,H2} where  {H1 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity}, 
-                                  H2 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity}}
-
-Alignment factor between point cloud populations, using either
-- a continuous density function cost: [`ApproxManifoldProducts.mmd`](@ref), or
-- a conventional iterative closest point (ICP) algorithm (when `.sample_count < 0`).
-
-This factor can support very large density clouds, with `sample_count` subsampling for individual alignments.
-
-Keyword Options:
-----------------
-- `sample_count::Int = 100`, number of subsamples to use during each alignment in `getSample`.  
-  - Values greater than 0 use MMD alignment, while values less than 0 use ICP alignment.
-- `bw::Real`, the bandwidth to use for [`mmd`](@ref) distance
-- `rescale::Real`
-- `N::Int`
-- `cvt::Function`, convert function for image when using `HeatmapGridDensity`.
-- `useStashing::Bool = false`, to switch serialization strategy to using [Stashing](@ref section_stash_unstash).
-- `dataEntry_cloud1::AbstractString = ""`, blob identifier used with stashing.
-- `dataEntry_cloud2::AbstractString = ""`, blob identifier used with stashing.
-- `dataStoreHint::AbstractString = ""`
-
-Example
--------
-```julia
-arp2 = ScatterAlignPose2(img1, img2, 2) # e.g. 2 meters/pixel 
-```
-
-Notes
------
-- Supports two belief "clouds" as either
-  - [`ManifoldKernelDensity`](@ref)s, or
-  - [`HeatmapGridDensity`](@ref)s.
-- Stanard `cvt` argument is lambda function to convert incoming images to user convention of image axes,
-  - **Geography map default** `cvt` flips image rows so that Pose2 +xy-axes corresponds to img[-x,+y]
-    - i.e. rows down is "North" and columns across from top left corner is "East".
-- Use rescale to resize the incoming images for lower resolution (faster) correlations
-- Both images passed to the construct must have the same type some matrix of type `T`.
-- Experimental support for Stashing based serialization.
-
-DevNotes:
----------
-- TODO Upgrade to use other information during alignment process, e.g. point normals for Pose3.
-
-See also: [`ScatterAlignPose2`](@ref), [`ScatterAlignPose3`](@ref), [`overlayScanMatcher`](@ref), [`Caesar._PCL.alignICP_Simple`](@ref).
-"""
-Base.@kwdef struct ScatterAlign{P,
-                                H1 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity},
-                                H2 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity} } <: IIF.AbstractManifoldMinimize
-  """ reference image for scan matching. """
-  cloud1::H1
-  """ test image to scan match against the reference image. """
-  cloud2::H2
-  """ Common grid scale for both images -- i.e. units/pixel.  
-  Constructor uses two arguments `gridlength`*`rescale=1`=`gridscale`.
-  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  = 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) """
-  useStashing::Bool = false
-  """ DataEntry ID for hollow store of cloud 1 & 2, TODO change to UUID instead """
-  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
-
-"""
-    ScatterAlignPose2(im1::Matrix, im2::Matrix, domain; options...)
-    ScatterAlignPose2(; mkd1::ManifoldKernelDensity, mkd2::ManifoldKernelDensity, moreoptions...)
-
-Specialization of [`ScatterAlign`](@ref) for [`Pose2`](@ref).
-
-See also: [`ScatterAlignPose3`](@ref)
-"""
-struct ScatterAlignPose2 <: IIF.AbstractManifoldMinimize
-  align::ScatterAlign{Pose2,<:Any,<:Any}
-end
-
-"""
-    ScatterAlignPose3(; cloud1=mkd1::ManifoldKernelDensity, 
-                        cloud2=mkd2::ManifoldKernelDensity, 
-                        moreoptions...)
-
-Specialization of [`ScatterAlign`](@ref) for [`Pose3`](@ref).
-
-See also: [`ScatterAlignPose2`](@ref)
-"""
-struct ScatterAlignPose3 <: IIF.AbstractManifoldMinimize
-  align::ScatterAlign{Pose3,<:Any,<:Any}
-end
 
 _getPoseType(::ScatterAlign{P}) where P = P
 
@@ -457,39 +350,6 @@ Base.show(io::IO, ::MIME"application/juno.inline", sap::ScatterAlignPose2) = sho
 ## =========================================================================================
 
 
-const _PARCHABLE_PACKED_CLOUD = Union{<:PackedManifoldKernelDensity, <:PackedHeatmapGridDensity}
-
-Base.@kwdef struct PackedScatterAlignPose2 <: AbstractPackedFactor
-  _type::String = "Caesar.PackedScatterAlignPose2"
-  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
-
-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{T}, 

diff --git a/ext/WeakdepsPrototypes.jl b/ext/WeakdepsPrototypes.jl
@@ -8,6 +8,43 @@ export generateCostAprilTagsPreimageCalib
 function drawBearingLinesAprilTags! end
 
 
+## ==============================================
+# CaesarImagesExt prototypes
+
+export applyMaskImage
+export makeMaskImage
+export makeMaskImages
+export imhcatPretty
+export toImage
+export writevideo, csmAnimationJoinImgs, csmAnimateSideBySide
+export makeVideoFromData
+
+export ScanMatcherPose2, PackedScanMatcherPose2
+export ScatterAlignPose2, PackedScatterAlignPose2
+export ScatterAlignPose3, PackedScatterAlignPose3
+
+export overlayScanMatcher
+export overlayScatter, overlayScatterMutate
+
+
+
+function applyMaskImage end
+function makeMaskImage end
+function makeMaskImages end
+function imhcatPretty end
+function toImage end
+
+function writevideo end
+function csmAnimationJoinImgs end
+function csmAnimateSideBySide end
+function makeVideoFromData end
+
+function overlayScanMatcher end
+
+function overlayScatter end
+function overlayScatterMutate end
+
+
 ## ==============================================
 # CaesarImageFeaturesExt prototypes