Common transform for ScatterAlign and _PCL , Pose3

src/3rdParty/_PCL/_PCL.jl

@@ -4,6 +4,7 @@
 module _PCL
 
 using ..Colors
+import ..Caesar: _FastTransform3D
 
 using Dates
 using DocStringExtensions
@@ -17,6 +18,7 @@ import Base: getindex, setindex!, resize!, cat, convert, sizeof, hasproperty, ge
 
 # gets overloaded
 import Manifolds: apply
+import IncrementalInference: ArrayPartition
 
 ## hold off on exports, users can in the mean-time use/import via e.g. _PCL.PointXYZ
 # export PointT, PointXYZ, PointXYZRGB, PointXYZRGBA

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

@@ -329,7 +329,7 @@ end
 # FIXME, to optimize, this function will likely be slow
 # TODO, consolidate with transformPointcloud(::ScatterAlign,..) function
 function apply( M_::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
-                rPp::Union{<:ProductRepr,<:Manifolds.ArrayPartition},
+                rPp::Manifolds.ArrayPartition,
                 pc::PointCloud{T} ) where T
   #
 
@@ -343,17 +343,15 @@ function apply( M_::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidea
                     sensor_orientation_=pc.sensor_orientation_ )
   #
 
-  rTp = affine_matrix(M_, rPp)
-  nc = size(rTp,1)-1
-  pV = MVector(zeros(nc)...,1.0)
-  _data = MVector(0.0,0.0,0.0,0.0)
+  ft3 = _FastTransform3D(M_, rPp, 0f0)
+  nc = M_ isa typeof(SpecialEuclidean(3)) ? 3 : 2
+  _data = MVector(0f0,0f0,0f0,0f0)
 
   # rotate the elements from the old point cloud into new static memory locations
   # NOTE these types must match the types use for PointCloud and PointXYZ
   # TODO not the world's fastest implementation
   for pt in pc.points
-    pV[1:nc] = pt.data[1:nc]
-    _data[1:nc] = (rTp*pV)[1:nc]
+    _data[1:nc] = ft3(pt.data[1:nc])
     _data[4] = pt.data[4]
     npt = PointXYZ(;color=pt.color, data=SVector{4,eltype(pt.data)}(_data...))
     push!(_pc.points, npt )

src/Caesar.jl

@@ -55,6 +55,7 @@ include("ExportAPI.jl")
 # and source files
 include("BearingRangeTrackingServer.jl")
 
+include("services/_FastTransform3D.jl")
 include("SlamServer.jl")
 include("DataUtils.jl")
 include("UserFunctions.jl")

src/UserFunctions.jl

@@ -10,40 +10,38 @@ relation.  Return the vector of points `aP_dest`.
 ````math
 {}^a \begin{bmatrix} x, y \end{bmatrix} = {}^a_b \mathbf{T} \, {}^b \begin{bmatrix} x, y \end{bmatrix}
 ````
+
+DevNotes
+- Consolidate functionality with [`_FastTransform3D`](@ref)
+
 """
-function _transformPointCloud2D!(
+function _transformPointCloud!(
     # manifold
-    M::typeof(SpecialEuclidean(2)),
+    M::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
     # destination points
     aP_dest::AbstractVector,
     # source points
     bP_src::AbstractVector,
     # transform coordinates
     aCb::AbstractVector{<:Real}; 
     # base point on manifold about which to do the transform
-    e0 = ProductRepr(SVector(0.0,0.0), SMatrix{2,2}(1.0,0.0,0.0,1.0)),
+    e0::ArrayPartition = getPointIdentity(M),
     backward::Bool=false
   )
   #
 
-  aTb = retract(M, e0, hat(M, e0, aCb))
-  aTb = backward ? inv(M,aTb) : aTb
-  bT_src = ProductRepr(MVector(0.0,0.0), SMatrix{2,2}(1.0,0.0,0.0,1.0))
-  aT_dest = ProductRepr(MVector(0.0,0.0), MMatrix{2,2}(1.0,0.0,0.0,1.0))
-
-  for (i,bP) in enumerate(bP_src)
-    bT_src.parts[1] .= bP
-    Manifolds.compose!(M, aT_dest, aTb, bT_src)
-    # aP_dest[i][:] = Manifolds.compose(M, aTb, bP_src[i]).parts[1]
-    aP_dest[i] .= aT_dest.parts[1]
-  end
+  aPb = retract(M, e0, hat(M, e0, aCb))
+  aPb = backward ? inv(M,aPb) : aPb
+  # can do 2d or 3d
+  aTb = _FastTransform3D(M,aPb)
+  aP_dest .= aTb.(bP_src)
 
   aP_dest
 end
 
-function _transformPointCloud2D(
+function _transformPointCloud(
     # manifold
-    M::typeof(SpecialEuclidean(2)),
+    M::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
     # source points
     bP_src::AbstractVector,
     # transform coordinates
@@ -52,9 +50,8 @@ function _transformPointCloud2D(
   )
   #
   #dest points
-  aP_dest = typeof(MVector(0.0,0.0))[MVector(0.0,0.0) for _ in 1:length(bP_src)] # Vector{typeof(MVector(0.0,0.0))}(undef, length(bP_src))
-  # fill!(aP_dest, MVector(0.0,0.0))
-  _transformPointCloud2D!(M, aP_dest, bP_src, aCb; kw...)
+  aP_dest = Vector{MVector{length(bP_src[1]),Float64}}(undef,length(bP_src)) 
+  _transformPointCloud!(M, aP_dest, bP_src, aCb; kw...)
 end
 
 

src/images/ScatterAlignPose2.jl

@@ -154,10 +154,11 @@ getManifold(::IIF.InstanceType{<:ScatterAlignPose2}) = getManifold(Pose2Pose2)
 getManifold(::IIF.InstanceType{<:ScatterAlignPose3}) = getManifold(Pose3Pose3)
 
 # runs once upon addFactor! and returns object later used as `cache`
-function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, fnc::ScatterAlignPose2)
+function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, fnc::Union{<:ScatterAlignPose2,<:ScatterAlignPose3})
   #
-  M = getManifold(Pose2)
-  e0 = ArrayPartition(SVector(0.0,0.0), SMatrix{2,2}(1.0, 0.0, 0.0, 1.0))
+  _getPoseType(sa::ScatterAlign{P}) where P = P
+  M = getManifold(_getPoseType(fnc.align))
+  e0 = getPointIdentity(M) # ArrayPartition(SVector(0.0,0.0), SMatrix{2,2}(1.0, 0.0, 0.0, 1.0))
 
   # reconstitute cloud belief from dataEntry
   for (va,de,cl) in zip(vars,[fnc.align.dataEntry_cloud1,fnc.align.dataEntry_cloud2],[fnc.align.cloud1,fnc.align.cloud2])
@@ -195,8 +196,8 @@ function getSample( cf::CalcFactor{<:ScatterAlignPose2} )
     qVj[i] .= sample(cf.factor.align.cloud2)[1][1]
   end
 
-  # qVi(qCp) = _transformPointCloud2D(M,pVi,qCp)
-  pVj(qCp) = _transformPointCloud2D(M,qVj,qCp; backward=true)
+  # qVi(qCp) = _transformPointCloud(M,pVi,qCp)
+  pVj(qCp) = _transformPointCloud(M,qVj,qCp; backward=true)
 
   # bw = SA[cf.factor.bw;]
   cost(xyr) = mmd(M.manifold[1], pVi, pVj(xyr), length(pVi), length(qVj), cf._allowThreads; cf.cache.bw)
@@ -276,8 +277,8 @@ function overlayScatter(sap::ScatterAlignPose2,
   best_coords = round.(best,digits=3)
 
   # transform points1 to frame of 2.  TBD, p as coordinate expansion point?
-  pts2T_u = _transformPointCloud2D(M, PT2, user_coords; backward=true)
-  pts2T_b = _transformPointCloud2D(M, PT2, best; backward=true)
+  pts2T_u = _transformPointCloud(M, PT2, user_coords; backward=true)
+  pts2T_b = _transformPointCloud(M, PT2, best; backward=true)
 
   @cast __pts1[i,j] := PT1[j][i]
   @cast __pts2[i,j] := PT2[j][i]

src/services/_FastTransform3D.jl

@@ -0,0 +1,45 @@
+
+
+"""
+    $TYPEDEF
+
+Common functor for transforming a large number of points through 
+`SpecialEuclidean(2)` or `SpecialEuclidean(3)` rigid body transform.
+
+Notes
+- Currently uses `Manifolds.affine_matrix`
+- Used by `_PCL.apply` and `ScatterAlign`
+"""
+struct _FastTransform3D{M,T}
+  rTo::SMatrix{4,4,T}
+end
+
+_FastTransform3D(M_::M, rPo::ArrayPartition, ::T=0.0) where {M <: typeof(SpecialEuclidean(3)),T<:Real} = _FastTransform3D{M,T}(SMatrix{4,4,T}(affine_matrix(M_,rPo)))
+
+function _FastTransform3D(M_::M, rPo::ArrayPartition, ::T=0.0) where {M <: typeof(SpecialEuclidean(2)),T<:Real}
+  rTo2 = affine_matrix(M_,rPo)
+  rTo = zeros(T,4,4)
+  rTo[4,4] = 1
+  rTo[1:2,1:2] .= rTo2[1:2,1:2]
+  rTo[1:2,4] .= rTo2[1:2,3]
+  _FastTransform3D{M,T}(SMatrix{4,4,T}(rTo))
+end
+
+function (ft3::_FastTransform3D{<:typeof(SpecialEuclidean(3)),T})(
+    src::AbstractVector{<:Real}
+  ) where T
+  #
+  pV = SVector{4,T}(src...,1.0)
+  wV = ft3.rTo*pV
+  SVector{3,T}(wV[1],wV[2],wV[3])
+end
+
+function (ft3::_FastTransform3D{<:typeof(SpecialEuclidean(2)),T})(
+    src::AbstractVector{<:Real}
+  ) where T
+  #
+  pV = SVector{4,T}(src...,0.0,1.0)
+  wV = ft3.rTo*pV
+  SVector{2,T}(wV[1], wV[2])
+end
+