diff --git a/Project.toml b/Project.toml index 1cc0feedb..1580e81b0 100644 --- a/Project.toml +++ b/Project.toml @@ -38,6 +38,7 @@ Reexport = "189a3867-3050-52da-a836-e630ba90ab69" Requires = "ae029012-a4dd-5104-9daa-d747884805df" RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" +Serialization = "9e88b42a-f829-5b0c-bbe9-9e923198166b" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" StatsBase = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91" @@ -98,7 +99,6 @@ PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" RoMEPlotting = "238d586b-a4bf-555c-9891-eda6fc5e55a2" RobotOS = "22415677-39a4-5241-a37a-00beabbbdae8" -Serialization = "9e88b42a-f829-5b0c-bbe9-9e923198166b" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1" diff --git a/src/3rdParty/_PCL/_PCL.jl b/src/3rdParty/_PCL/_PCL.jl index 185bd7d0f..1d20d83ca 100644 --- a/src/3rdParty/_PCL/_PCL.jl +++ b/src/3rdParty/_PCL/_PCL.jl @@ -11,6 +11,7 @@ using Dates using Printf using DocStringExtensions using Requires +using Serialization # FIXME REMOVE, only used for legacy getDataPointCloud using StaticArrays using Statistics using StatsBase @@ -35,6 +36,7 @@ import IncrementalInference: ArrayPartition include("entities/PCLTypes.jl") # bring in further source code include("services/PointCloud.jl") +include("services/PointCloudUtils.jl") include("services/ConsolidateRigidTransform.jl") include("services/ICP_Simple.jl") diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl index e34597186..6abf138c3 100644 --- a/src/3rdParty/_PCL/services/PointCloud.jl +++ b/src/3rdParty/_PCL/services/PointCloud.jl @@ -374,6 +374,19 @@ function _filterMinRange( pts[msk] end +function _prepPointCloud( + pc_a::PointCloud, + dims=1:3; + minrange=0.0, + maxrange=999.0 +) + pts_a_ = (s->s.data[dims]).(pc_a.points) + pts_a = _PCL._filterMinRange(pts_a_, minrange, maxrange) + # pts_a = (s->[s.x;s.y;s.z]).(pc_a.points) + # To convert them to a matrix. + @cast pts_matrix[i,d] := pts_a[i][d] + return collect(pts_matrix) +end ## ========================================================================================================= ## Custom printing diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl new file mode 100644 index 000000000..bbd3b9f8a --- /dev/null +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -0,0 +1,76 @@ + +export getDataPointCloud, getPointCloud, getPointCloud2D, getPointCloud3D + + +function getDataPointCloud( + nfg::AbstractDFG, + varlbl, + pattern::Union{Symbol,UUID,<:AbstractString, Regex} +) + # get point cloud blob + try + de,dbl = getData(nfg, Symbol(varlbl), pattern) + if isnothing(dbl) + @error "could find in variable $varlbl, blob $pattern" + return nothing + end + # FIXME, change serialization to more standard pcd or laz formats + return dbl |> IOBuffer |> Serialization.deserialize + catch err + if err isa KeyError + @error err + return nothing + end + throw(err) + end + # unpack specific blob format +end + + + +function getPointCloud( + nfg::AbstractDFG, + label, + pattern = r"PCLPointCloud2", + w...; + kw... +) + pc2_a = getDataPointCloud(nfg, label, pattern) + if pc2_a isa Nothing + return nothing + end + pc_a = _PCL.PointCloud(pc2_a) + return _prepPointCloud(pc_a, w...; kw...) +end + +getPointCloud2D( + nfg::AbstractDFG, + label, + regex=r"PCLPointCloud2", + dims=1:2; + minrange=0.0, + maxrange=999. +) = getPointCloud( + nfg, + label, + regex, + dims; + minrange, + maxrange +) + +getPointCloud3D( + nfg::AbstractDFG, + label, + regex=r"PCLPointCloud2", + dims=1:3; + minrange=0.0, + maxrange=999. +) = getPointCloud( + nfg, + label, + regex, + dims; + minrange, + maxrange +)