Merge pull request #952 from JuliaRobotics/23Q1/enh/imagefeatures

add ImageFeatures utils for ORB

Author: dehann

Project.toml

@@ -8,6 +8,7 @@ version = "0.14.0"
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
 Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
+ColorVectorSpace = "c3611d14-8923-5661-9e6a-0046d554d3a4"
 Combinatorics = "861a8166-3701-5b0c-9a16-15d98fcdc6aa"
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
 DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
@@ -20,13 +21,17 @@ DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 FFTW = "7a1cc6ca-52ef-59f5-83cd-3a7055c09341"
 FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326"
+GeometricalPredicates = "fd0ad045-b25c-564e-8f9c-8ef5c5f21267"
 ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 ImageMagick = "6218d12a-5da1-5696-b52f-db25d2ecc6d1"
 IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
+Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59"
 JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
 JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6"
 JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3"
+JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
+LasIO = "570499db-eae3-5eb6-bdd5-a5326f375e68"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 MultivariateStats = "6f286f6a-111f-5878-ab1e-185364afe411"
@@ -55,22 +60,27 @@ YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6"
 ApproxManifoldProducts = "0.6"
 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.18, 0.19"
+DistributedFactorGraphs = "0.19, 0.20"
 Distributions = "0.25"
 DocStringExtensions = "0.8, 0.9"
 FFTW = "1"
 FileIO = "1"
 GeometryBasics = "0.4"
+GeometricalPredicates = "0.4"
 ImageCore = "0.8, 0.9"
 ImageMagick = "1"
-IncrementalInference = "0.31, 0.32"
+IncrementalInference = "0.31, 0.32, 0.33"
+Interpolations = "0.14"
 JLD2 = "0.3, 0.4"
 JSON = "0.20, 0.21"
 JSON2 = "0.3, 0.4"
+JSON3 = "1"
 KernelDensityEstimate = "0.5"
+LasIO = "0.3"
 Manifolds = "0.8"
 MultivariateStats = "0.10"
 NLsolve = "4"
@@ -79,7 +89,7 @@ Optim = "1"
 ProgressMeter = "1"
 Reexport = "1"
 Requires = "1"
-RoME = "0.21"
+RoME = "0.21, 0.22"
 Rotations = "1.1"
 StaticArrays = "1"
 StatsBase = "0.33"
@@ -98,7 +108,7 @@ Downloads = "f43a241f-c20a-4ad4-852c-f6b1247861c6"
 FixedPointNumbers = "53c48c17-4a7d-5ca2-90c5-79b7896eea93"
 GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231"
 Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0"
-LasIO = "570499db-eae3-5eb6-bdd5-a5326f375e68"
+ImageFeatures = "92ff4b2b-8094-53d3-b29d-97f740f06cef"
 PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 RoMEPlotting = "238d586b-a4bf-555c-9891-eda6fc5e55a2"
@@ -107,4 +117,4 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [targets]
-test = ["AprilTags", "BSON", "DelimitedFiles", "Downloads", "FixedPointNumbers", "GraphPlot", "Images", "LasIO", "PyCall", "Random", "RobotOS", "Test", "ZMQ"]
+test = ["AprilTags", "BSON", "DelimitedFiles", "Downloads", "FixedPointNumbers", "GraphPlot", "Images", "PyCall", "Random", "RobotOS", "Test", "ZMQ"]

src/3rdParty/_PCL/_PCL.jl

@@ -8,6 +8,7 @@ import ..Caesar: _FastTransform3D
 import ..Caesar: homogeneous_coord_to_euler_coord, euler_coord_to_homogeneous_coord, euler_angles_to_linearized_rotation_matrix
 import ..Caesar: _SE3_MANI # create_homogeneous_transformation_matrix
 import ..Caesar: getBelief, mean, calcPPE
+import ..Caesar: AbstractBoundingBox, OrientedBoundingBox, AxisAlignedBoundingBox, inside
 
 using FileIO
 using Colors
@@ -22,11 +23,11 @@ using LinearAlgebra
 using NearestNeighbors
 using Manifolds
 import Rotations as _Rot
-import GeometryBasics as GeoB # name collisions on members: Point, etc.
 using DistributedFactorGraphs
 using TensorCast
 using UUIDs
 using MultivariateStats
+using LasIO
 
 # FIXME REMOVE, only used for legacy getDataPointCloud
 using Serialization 
@@ -48,17 +49,17 @@ import IncrementalInference: ArrayPartition
 
 # bring in the types
 include("entities/PCLTypes.jl")
-include("entities/OtherTypes.jl")
 # bring in further source code
 include("services/GeomBasicsUtils.jl")
 include("services/PointCloud.jl")
+include("services/LasIOSupport.jl")
 include("services/PointCloudUtils.jl")
 include("services/ConsolidateRigidTransform.jl")
 include("services/ICP_Simple.jl")
 
 
 function __init__()
-  @require LasIO="570499db-eae3-5eb6-bdd5-a5326f375e68" include("services/LasIOSupport.jl")
+  # @require LasIO="570499db-eae3-5eb6-bdd5-a5326f375e68" include("services/LasIOSupport.jl")
   @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("services/ROSConversions.jl")
   # moved plotting out of Caesar, use Arena.jl or RoMEPlotting.jl instead
 end

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

@@ -1,74 +1,7 @@
 # Utils based on GeometryBasics
 
 
-"""
-    $SIGNATURES
-
-Get the eight corners of bounding box in it's local coordinates.
-
-See also: [`plotBoundingBox`](@ref)
-"""
-function getCorners(
-  BB::_PCL.AbstractBoundingBox
-)
-  # https://math.stackexchange.com/questions/1472049/check-if-a-point-is-inside-a-rectangular-shaped-area-3d
-  hr = BB.hr
-  x_, y_, z_ = hr.origin[1], hr.origin[2], hr.origin[3]
-  dx, dy, dz = hr.widths[1], hr.widths[2], hr.widths[3]
 
-  p1 = [x_;y_;z_]
-  p2 = [x_;y_+dy;z_]
-  p4 = [x_+dx;y_;z_]
-  p3 = [x_+dx;y_+dy;z_]
-
-  p5 = [x_;y_;z_+dz]
-  p6 = [x_;y_+dy;z_+dz]
-  p8 = [x_+dx;y_;z_+dz]
-  p7 = [x_+dx;y_+dy;z_+dz]
-
-  corners_ = [p1,p2,p3,p4,p5,p6,p7,p8]
-
-  _r_H_bb(::_PCL.AxisAlignedBoundingBox) = SMatrix{4,4}(diagm(ones(4)))
-  _r_H_bb(obb::_PCL.OrientedBoundingBox) = inv(obb.bb_H_r)
-  r_H_bb = _r_H_bb(BB)
-  (s->(r_H_bb*[s...;1.])[1:3]).(corners_)
-end
-
-"""
-    $SIGNATURES
-
-Is point p inside the HyperRectangle.
-"""
-function inside(
-  hr::GeoB.HyperRectangle, 
-  p #::GeoB.Point
-)
-  #
-  _p0,_p1 = GeoB.minmax(GeoB.Point(p...), hr.origin, hr.origin + hr.widths)
-  _p0 == hr.origin && hr.widths == (_p1-_p0)
-end
-
-# inside(
-#   aabb::AxisAlignedBoundingBox, 
-#   p
-# ) = inside(aabb.hr, p)
-
-
-function inside(
-  obb::AbstractBoundingBox, 
-  pt
-)
-# https://math.stackexchange.com/questions/1472049/check-if-a-point-is-inside-a-rectangular-shaped-area-3d
-  p = getCorners(obb)
-  
-  _i = p[2] - p[1]
-  _j = p[4] - p[1]
-  _k = p[5] - p[1]
-  _v = pt   - p[1]
-
-  # check if inside cuboid
-  (0 < _v'*_i < _i'*_i) && (0 < _v'*_j < _j'_j) && (0 < _v'*_k < _k'*_k)
-end
 
 
 """
@@ -82,7 +15,7 @@ function getSubcloud(
   minrange::Real=0, 
   maxrange::Real=999
 )
-  objmask = map(s->_PCL.inside(mask, s.data[1:3]), pc_full.points)
+  objmask = map(s->inside(mask, s.data[1:3]), pc_full.points)
   spt = (s->s.data[1:3]).(pc_full.points[objmask])
   if 0 == length(spt)
     return _PCL.PointCloud()

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

@@ -1,11 +1,11 @@
 # support saving and loading PointClouds as las files
 
-@info "Caesar._PCL is loading features using LasIO.jl"
+# @info "Caesar._PCL is loading features using LasIO.jl"
 
-using .LasIO
-using .FileIO
-using .Dates
-using .TensorCast
+# using .LasIO
+# using .FileIO
+# using .Dates
+# using .TensorCast
 
 
 function loadLAS(

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

@@ -8,6 +8,7 @@ function getDataPointCloud(
   nfg::AbstractDFG,
   varlbl, 
   pattern::Union{Symbol,UUID,<:AbstractString, Regex};
+  format::Symbol = :las,
   getDatakws...
 )
   # get point cloud blob
@@ -17,8 +18,14 @@ function getDataPointCloud(
       @error "could find in variable $varlbl, blob $pattern"
       return nothing
     end
-    # FIXME, change serialization to more standard pcd or laz formats, see Caesar.jl #921
-    return dbl |> IOBuffer |> Serialization.deserialize
+    if format == :las
+      dbl |> IOBuffer |> loadLAS
+    elseif format == :Serialization
+      # FIXME, change serialization to more standard pcd or laz formats, see Caesar.jl #921
+      return dbl |> IOBuffer |> Serialization.deserialize
+    else
+      error("Unknown point cloud blob format $format")
+    end
   catch err
     if err isa KeyError
       @error err

src/Caesar.jl

@@ -8,6 +8,7 @@ import DistributedFactorGraphs: getManifold
 
 # handy project consts (not for export)
 import IncrementalInference: NothingUnion, InstanceType
+import GeometricalPredicates as GeoPr
 
 using Requires
 using Dates
@@ -19,6 +20,7 @@ using StaticArrays
 import Rotations as _Rot
 import GeometryBasics as GeoB
 
+
 # TODO remove
 const _Rotations = _Rot
 
@@ -33,6 +35,7 @@ using
   CoordinateTransformations,
   JSON,
   JSON2,
+  JSON3,
   UUIDs,
   Base64,
   FileIO,
@@ -65,6 +68,8 @@ include("transforms/entities/TransformTypes.jl")
 include("transforms/services/HomographyTransforms.jl")
 include("transforms/services/_FastTransform3D.jl")
 
+include("entities/OtherTypes.jl")
+include("services/PointUtils.jl")
 include("services/DataUtils.jl")
 include("services/UserFunctions.jl")
 
@@ -101,6 +106,7 @@ function __init__()
   @require ImageMagick="6218d12a-5da1-5696-b52f-db25d2ecc6d1" include("images/imagedata.jl")
   @require Images="916415d5-f1e6-5110-898d-aaa5f9f070e0" begin 
     include("images/images.jl")
+    include("images/ImageToVideoUtils.jl")
     include("images/ScanMatcherUtils.jl")
     include("images/ScanMatcherPose2.jl")
     include("images/ScatterAlignPose2.jl")
@@ -109,6 +115,7 @@ function __init__()
     # @require Gadfly="c91e804a-d5a3-530f-b6f0-dfbca275c004" include("plotting/ScatterAlignPlotting.jl")
     @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("images/ROSConversions.jl")
   end
+  @require ImageFeatures="92ff4b2b-8094-53d3-b29d-97f740f06cef" include("images/imagefeatures.jl")
   @require Distributed="8ba89e20-285c-5b6f-9357-94700520ee1b" include("images/DistributedUtils.jl")
 end
 

src/ExportAPI.jl

@@ -47,3 +47,6 @@ export
   phaseShiftSingle!,
   liebf!,
   SASDebug
+
+export calcPointsInPoly
+export inside, AxisAlignedBoundingBox, OrientedBoundingBox

src/beamforming/SASBearing2D.jl

@@ -227,8 +227,45 @@ mutable struct PackedSASBearing2D <: DFG.AbstractPackedFactor
     waveformsInReal::Vector{Float64}
     waveformsInIm::Vector{Float64}
     PackedSASBearing2D() = new()
-    PackedSASBearing2D(rangemodel::String, tp::Int, wavedataRawV::Vector{Float64}, wavedataRawV_dim::Int, cfgJson::String, cfgTotal::String, cfgLIE::String, waveformsInReal::Vector{Float64}, waveformsInIm::Vector{Float64}) = new(rangemodel, tp, wavedataRawV, wavedataRawV_dim, cfgJson, cfgTotal, cfgLIE, waveformsInReal, waveformsInIm)
-    PackedSASBearing2D(rangemodel::String, tp::Int, wd::Array{Float64,2}, cf::Dict, cfgTotal::String, cfgLIE::String, waveformsInReal::Vector{Float64}, waveformsInIm::Vector{Float64}) = new(rangemodel, tp, wd[:], size(wd,1), JSON2.write(cf), cfgTotal, cfgLIE, waveformsInReal, waveformsInIm)
+    PackedSASBearing2D(
+      rangemodel::String, 
+      tp::Int, 
+      wavedataRawV::Vector{Float64}, 
+      wavedataRawV_dim::Int, 
+      cfgJson::String, 
+      cfgTotal::String, 
+      cfgLIE::String, 
+      waveformsInReal::Vector{Float64}, 
+      waveformsInIm::Vector{Float64}) = new(
+        rangemodel, 
+        tp, 
+        wavedataRawV, 
+        wavedataRawV_dim, 
+        cfgJson, 
+        cfgTotal, 
+        cfgLIE, 
+        waveformsInReal, 
+        waveformsInIm
+      )
+    PackedSASBearing2D(
+      rangemodel::String, 
+      tp::Int, 
+      wd::Array{Float64,2}, 
+      cf::Dict, 
+      cfgTotal::String, 
+      cfgLIE::String, 
+      waveformsInReal::Vector{Float64}, 
+      waveformsInIm::Vector{Float64}) = new(
+        rangemodel, 
+        tp, 
+        wd[:], 
+        size(wd,1), 
+        JSON2.write(cf), 
+        cfgTotal, 
+        cfgLIE, 
+        waveformsInReal, 
+        waveformsInIm
+      )
 end
 
 # Note: Need this otherwise it uses a default converter and causes type conversion issues.