diff --git a/Project.toml b/Project.toml index 3949e4b9f..a8f5519f1 100644 --- a/Project.toml +++ b/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" diff --git a/src/3rdParty/_PCL/_PCL.jl b/src/3rdParty/_PCL/_PCL.jl index 22bc870e8..0957362c8 100644 --- a/src/3rdParty/_PCL/_PCL.jl +++ b/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 diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl index 6a7801097..0d9a8e0c7 100644 --- a/src/3rdParty/_PCL/services/PointCloud.jl +++ b/src/3rdParty/_PCL/services/PointCloud.jl @@ -483,4 +483,6 @@ Base.show(io::IO, ::MIME"application/prs.juno.inline", pc::PointCloud) = show(io + + # \ No newline at end of file diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 211f7fac5..954ebe0b0 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/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, diff --git a/src/ExportAPI.jl b/src/ExportAPI.jl index ea139893a..a7a9a1146 100644 --- a/src/ExportAPI.jl +++ b/src/ExportAPI.jl @@ -49,4 +49,5 @@ export SASDebug export calcPointsInPoly -export inside, AxisAlignedBoundingBox, OrientedBoundingBox \ No newline at end of file +export inside, AxisAlignedBoundingBox, OrientedBoundingBox +export packBlob, unpackBlob \ No newline at end of file diff --git a/src/images/imagedata.jl b/src/images/imagedata.jl index 5160c42fb..1397167ef 100644 --- a/src/images/imagedata.jl +++ b/src/images/imagedata.jl @@ -18,8 +18,10 @@ 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) @@ -27,8 +29,27 @@ function toFormat(format::DataType, 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 diff --git a/src/images/imagedraw.jl b/src/images/imagedraw.jl index d23bf6d02..a3ac0d841 100644 --- a/src/images/imagedraw.jl +++ b/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 \ No newline at end of file diff --git a/src/images/images.jl b/src/images/images.jl index cc091a292..06f572d4c 100644 --- a/src/images/images.jl +++ b/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