@@ -54,14 +54,16 @@ getPointCloud2D(
5454 regex= r" PCLPointCloud2"
5555 dims= 1 : 2 ;
5656 minrange= 0.0 ,
57- maxrange= 999.
57+ maxrange= 999. ,
58+ kw...
5859) = getPointCloud (
5960 nfg,
6061 label,
6162 regex,
6263 dims;
6364 minrange,
64- maxrange
65+ maxrange,
66+ kw...
6567)
6668
6769getPointCloud3D (
@@ -70,14 +72,16 @@ getPointCloud3D(
7072 regex= r" PCLPointCloud2"
7173 dims= 1 : 3 ;
7274 minrange= 0.0 ,
73- maxrange= 999.
75+ maxrange= 999. ,
76+ kw...
7477) = getPointCloud (
7578 nfg,
7679 label,
7780 regex,
7881 dims;
7982 minrange,
80- maxrange
83+ maxrange,
84+ kw...
8185)
8286
8387"""
@@ -98,6 +102,25 @@ function getDataSubcloudLocal(
98102 getSubcloud (pointcloud, p_BBo)
99103end
100104
105+ function getPointCloudInWorld (
106+ dfg:: AbstractDFG ,
107+ vlb:: Symbol ;
108+ solveKey:: Symbol = :default ,
109+ blobLabel:: Regex = r" PCLPointCloud2"
110+ checkhash:: Bool = false
111+ )
112+ #
113+ M = SpecialEuclidean (3 )
114+ e0 = ArrayPartition (SA[0 ;0 ;0. ], SMatrix {3,3} (1 ,0 ,0 ,0 ,1 ,0 ,0 ,0 ,1. ))
115+ p_PC = getDataPointCloud (dfg, vlb, blobLabel; checkhash) |> PointCloud
116+ w_Cp = getPPESuggested (dfg, vlb, solveKey)
117+ wTp = exp (M,e0,hat (M,e0,w_Cp))
118+ # wTp = ArrayPartition(SVector(wTp_.x[1]), SMatrix{3,3}(wTp_.x[2]))
119+ w_PC = apply (M, wTp, p_PC)
120+
121+ w_PC
122+ end
123+
101124"""
102125 $SIGNATURES
103126
@@ -164,7 +187,7 @@ function alignPointCloudLOO!(
164187 loo_idx:: Int ;
165188 updateTloo:: Bool = false ,
166189 verbose= false ,
167- max_iterations = 25 ,
190+ max_iterations = 40 ,
168191 correspondences = 500 ,
169192 neighbors = 50
170193)
@@ -241,7 +264,8 @@ function alignPointCloudsLOOIters!(
241264 l_PCs:: AbstractVector{<:_PCL.PointCloud} ,
242265 updateTloo:: Bool = true ,
243266 MC:: Int = 3 ;
244- iterLength:: Int = length (l_PCs)
267+ iterLength:: Int = length (l_PCs),
268+ lookws...
245269)
246270 o_Ts_l_ = (updateTloo ? s-> s : deepcopy)(o_Ts_l)
247271
@@ -250,7 +274,8 @@ function alignPointCloudsLOOIters!(
250274 o_Ts_l_,
251275 l_PCs,
252276 k;
253- updateTloo= true
277+ updateTloo= true ,
278+ lookws...
254279 )
255280 end
256281
@@ -271,33 +296,71 @@ function findObjectVariablesFromWorld(
271296 r_BBo:: _PCL.AbstractBoundingBox ;
272297 solveKey:: Symbol = :default ,
273298 minpoints:: Int = 5000 ,
274- limit:: Int = 50 ,
299+ limit:: Int = 10 ,
300+ selection:: Symbol = :biggest ,
275301 varPattern:: Regex = r" x\d +"
276302 tags:: AbstractVector{Symbol} = Symbol[],
277- varList:: AbstractVector{Symbol} = sort (ls (dfg,varPattern;tags); lt= DFG. natural_lt),
278- blobLabel = r" PCLPointCloud2"
303+ minList:: Int = 1 ,
304+ maxList:: Int = 999999 ,
305+ minrange= 1.0 ,
306+ varList:: AbstractVector{Symbol} = sort (ls (dfg,varPattern;tags); lt= DFG. natural_lt)[minList: maxList],
307+ blobLabel = r" PCLPointCloud2"
308+ sortList:: Bool = true ,
309+ checkhash:: Bool = false
279310)
280311 M = SpecialEuclidean (3 )
281- objVars = Symbol[]
282-
283- for vlb in varList
284- b_PC = _PCL. getDataPointCloud (dfg, vlb, blobLabel; checkhash= false ) |> _PCL. PointCloud
285- # TODO use PPE after PPEs are updated to Manifolds representation
286- r_T_b = getBelief (dfg, vlb, solveKey) |> mean
287- r_PC = _PCL. apply (M, r_T_b, b_PC)
288- sc = _PCL. getSubcloud (r_PC, r_BBo)
289- if minpoints < length (sc)
290- push! (objVars, vlb)
312+ len = length (varList)
313+ objVars_ = Vector {Symbol} (undef, len)
314+ numPts_ = Vector {Int} (undef, len)
315+ tsk = Vector {Task} (undef, len)
316+
317+ for (i,vlb) in enumerate (varList)
318+ tsk[i] = Threads. @spawn begin
319+ # b_PC = _PCL.getDataPointCloud(dfg, $vlb, blobLabel; checkhash=false) |> _PCL.PointCloud
320+ b_PC = _PCL. getPointCloud3D (dfg, $ vlb, blobLabel; checkhash, minrange) |> _PCL. PointCloud
321+ # TODO use PPE after PPEs are updated to Manifolds representation
322+ r_T_b = getBelief (dfg, $ vlb, solveKey) |> mean
323+ r_PC = _PCL. apply (M, r_T_b, b_PC)
324+ sc = _PCL. getSubcloud (r_PC, r_BBo)
325+ numpts = length (sc)
326+ if minpoints < numpts
327+ objVars_[$ i] = $ vlb
328+ numPts_[$ i] = numpts
329+ end
291330 end
292331 end
332+ wait .(tsk)
293333
334+ objVars = Vector {Symbol} ()
335+ numPts = Vector {Int} ()
336+
337+ for i in 1 : len
338+ if isassigned (objVars_, i)
339+ push! (objVars, objVars_[i])
340+ push! (numPts, numPts_[i])
341+ end
342+ end
294343 len = length (objVars)
295- if 0 < len
296- idx = unique (Int .(round .(1 : len/ limit: len)))
297- objVars[idx]
344+
345+ ovlbs = if 0 < len
346+ if selection == :spread
347+ # evenly select poses to object (assuming enough coverage)
348+ idx = unique (Int .(round .(1 : len/ limit: len)))
349+ objVars[idx]
350+ elseif selection == :biggest
351+ prm = sortperm (numPts; rev = true )
352+ idx = 1 : minimum ([len; limit])
353+ objVars[prm[idx]]
354+ end
298355 else
299356 objVars
300357 end
358+ # usually sorted list is beter
359+ if sortList
360+ sort (ovlbs; lt= DFG. natural_lt)
361+ else
362+ ovlbs
363+ end
301364end
302365
303366#
@@ -334,6 +397,10 @@ function calcAxes3D(
334397 flipXY:: Bool = false
335398)
336399 #
400+ if 0 === length (pc)
401+ @warn " Cannot calculate a default axis for empty point cloud"
402+ return ArrayPartition (SVector (0 ,0 ,0. ), SMatrix {3,3} (1 ,0 ,0 ,0 ,1 ,0 ,0 ,0 ,1. ))
403+ end
337404 xyz_ = (p-> [p. x;p. y;p. z]). (pc. points)
338405 @cast xyz[d,i] := xyz_[i][d]
339406 mdl = fit (PCA, xyz; pratio= 1 )
@@ -352,11 +419,49 @@ function calcAxes3D(
352419 R0 = SMatrix {3,3,Float64} (pc. sensor_orientation_)
353420 Rx = _Rot. RotX (pi )* R
354421 Ry = _Rot. RotY (pi )* R
355- costs = [distance (Mr, R0, R); distance (Mr, R0, Rx); distance (Mr, R0, Ry)]
356- (R,Rx,Ry)[argmin (costs)]
422+ costs = Float64[]
423+ arr = Matrix{Float64}[]
424+ for r in [R, Rx, Ry]
425+ try
426+ push! (costs, distance (Mr, R0, r))
427+ push! (arr, r)
428+ catch e
429+ # ERROR: ArgumentError: invalid index: nothing of type Nothing
430+ # [6] log!(M::Rotations{3}, X::MMatrix{3, 3, Float64, 9}, p::SMatrix{3, 3, Float64, 9}, q::Matrix{Float64})
431+ # @ Manifolds ~/.julia/packages/Manifolds/7JKQJ/src/manifolds/GeneralUnitaryMatrices.jl:560
432+ e isa ArgumentError ? nothing : rethrow (e)
433+ end
434+ end
435+ # costs = [distance(Mr, R0, R); distance(Mr, R0, Rx); distance(Mr, R0, Ry)]
436+ arr[argmin (costs)]
357437 end
358438
359439 ArrayPartition (SVector (μ... ), SMatrix {3,3} (R_enh))
360440end
361441
442+ function Base. convert (
443+ :: Type{<:PointXYZ{C,F1}} ,
444+ pt:: PointXYZ{C,F2}
445+ ) where {C, F1 <: Real , F2 <: Real }
446+ #
447+ PointXYZ (;
448+ color= pt. color,
449+ data= SVector {4,F1} (pt. data... )
450+ )
451+ end
452+
453+
454+ function Base. convert (
455+ :: Type{<:PointCloud{<:PointXYZ{C,F1},S,M}} ,
456+ pc:: PointCloud{<:PointXYZ{C,F2}}
457+ ) where {C, F1 <: Real , F2 <: Real , S, M}
458+ #
459+ if 0 == length (pc)
460+ return PointCloud {PointXYZ{C, F1}, S, M} ()
461+ end
462+ xyz_ = (p-> F1[F1 (p. x);F1 (p. y);F1 (p. z)]). (pc. points)
463+ @cast xyz[i,d] := xyz_[i][d]
464+ PointCloud (xyz)
465+ end
466+
362467#
0 commit comments