getPointCloud util functions

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"
 

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")
 

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

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
+)