Merge pull request #929 from JuliaRobotics/22Q4/enh/objaffsubcloud

ObjectAffordanceSubcloud Factor

Author: dehann

Project.toml

@@ -19,6 +19,7 @@ Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 FFTW = "7a1cc6ca-52ef-59f5-83cd-3a7055c09341"
 FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
+GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326"
 ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 ImageMagick = "6218d12a-5da1-5696-b52f-db25d2ecc6d1"
 IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
@@ -92,7 +93,6 @@ BSON = "fbb218c0-5317-5bc6-957e-2ee96dd4b1f0"
 DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab"
 Downloads = "f43a241f-c20a-4ad4-852c-f6b1247861c6"
 FixedPointNumbers = "53c48c17-4a7d-5ca2-90c5-79b7896eea93"
-Gadfly = "c91e804a-d5a3-530f-b6f0-dfbca275c004"
 GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231"
 Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0"
 PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0"
@@ -103,4 +103,4 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [targets]
-test = ["AprilTags", "BSON", "DelimitedFiles", "Downloads", "FixedPointNumbers", "Gadfly", "GraphPlot", "Images", "PyCall", "Random", "RobotOS", "Test", "ZMQ"]
+test = ["AprilTags", "BSON", "DelimitedFiles", "Downloads", "FixedPointNumbers", "GraphPlot", "Images", "PyCall", "Random", "RobotOS", "Test", "ZMQ"]

examples/dev/icp/simpleicp.jl

@@ -191,7 +191,8 @@ function estimate_rigid_body_transformation(x_fix, y_fix, z_fix, nx_fix, ny_fix,
 
     t = x[4:6]
 
-    H = create_homogeneous_transformation_matrix(R, t)
+    H = affine_matrix(_SE3_MANI, ArrayPartition(t, R))
+    # H = create_homogeneous_transformation_matrix(R, t)
 
     return H, residuals
 
@@ -205,12 +206,6 @@ function euler_angles_to_linearized_rotation_matrix(α1, α2, α3)
 
 end
 
-function create_homogeneous_transformation_matrix(R, t)
-
-    H = [R          t
-         zeros(1,3) 1]
-
-end
 
 function euler_coord_to_homogeneous_coord(XE)
 

src/3rdParty/_PCL/_PCL.jl

@@ -4,7 +4,9 @@
 module _PCL
 
 # using ..Colors
-import ..Caesar: _FastTransform3D, homogeneous_coord_to_euler_coord, euler_coord_to_homogeneous_coord, euler_angles_to_linearized_rotation_matrix, create_homogeneous_transformation_matrix
+import ..Caesar: _FastTransform3D
+import ..Caesar: homogeneous_coord_to_euler_coord, euler_coord_to_homogeneous_coord, euler_angles_to_linearized_rotation_matrix
+import ..Caesar: _SE3_MANI # create_homogeneous_transformation_matrix
 
 using Colors
 using Dates
@@ -18,11 +20,13 @@ using LinearAlgebra
 using NearestNeighbors
 using Manifolds
 import Rotations as _Rot
+import GeometryBasics as GeoB # name collisions on members: Point, etc.
 using DistributedFactorGraphs
 using TensorCast
 using UUIDs
 
-using Serialization # FIXME REMOVE, only used for legacy getDataPointCloud
+# FIXME REMOVE, only used for legacy getDataPointCloud
+using Serialization 
 
 # Going to add dispatches on these functions
 import Base: getindex, setindex!, resize!, cat, convert, sizeof, hasproperty, getproperty
@@ -33,12 +37,14 @@ 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
+# export inside, getSubcloud
 # export PCLHeader, PointCloud
 
 
 # bring in the types
 include("entities/PCLTypes.jl")
 # bring in further source code
+include("services/GeomBasicsUtils.jl")
 include("services/PointCloud.jl")
 include("services/PointCloudUtils.jl")
 include("services/ConsolidateRigidTransform.jl")
@@ -47,7 +53,7 @@ include("services/ICP_Simple.jl")
 
 function __init__()
   @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("services/ROSConversions.jl")
-  @require Gadfly="c91e804a-d5a3-530f-b6f0-dfbca275c004" include("services/PlottingUtilsPCL.jl")
+  # moved plotting out of Caesar, use Arena.jl or RoMEPlotting.jl instead
 end
 
 

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

@@ -1,4 +1,59 @@
 
+@doc raw"""
+    $SIGNATURES
+
+Transform and put 2D pointcloud as [x,y] coordinates of a-frame into `aP_dest`, by transforming 
+incoming b-frame points `bP_src` as [x,y] coords via the transform `aTb` describing the b-to-a (aka a-in-b)
+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 _transformPointCloud!(
+  # manifold
+  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::ArrayPartition = getPointIdentity(M),
+  backward::Bool=false
+)
+  #
+  
+  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 _transformPointCloud(
+  # manifold
+  M::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
+  # source points
+  bP_src::AbstractVector,
+  # transform coordinates
+  aCb::AbstractVector{<:Real}; 
+  kw...
+)
+  #
+  #dest points
+  aP_dest = Vector{MVector{length(bP_src[1]),Float64}}(undef,length(bP_src)) 
+  _transformPointCloud!(M, aP_dest, bP_src, aCb; kw...)
+end
+
+
 
 ## ============================================================
 ## Consolidated rigid transform standardized against Manifolds.jl
@@ -8,9 +63,11 @@
 # Works for transform of both 2D and 3D  point clouds
 # FIXME, to optimize, this function will likely be slow
 # TODO, consolidate with transformPointcloud(::ScatterAlign,..) function
-function Manifolds.apply( M_::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
-                rPp::Manifolds.ArrayPartition,
-                pc::PointCloud{T} ) where T
+function Manifolds.apply( 
+  M_::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
+  rPp::Manifolds.ArrayPartition,
+  pc::PointCloud{T} 
+) where T
   #
 
   # allocate destination

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

@@ -0,0 +1,67 @@
+# Utils based on GeometryBasics
+
+# 
+function transformFromWorldToLocal(
+  dfg::AbstractDFG,
+  vlbl::Symbol,
+  w_BBo::GeoB.HyperRectangle;
+  solveKey=:default
+)
+  #  
+  v = getVariable(dfg, vlbl)
+  # vt = getVariableType(v)
+  M = getManifold(v)
+  e0 = ArrayPartition(SA[1;1;1.], SMatrix{3,3}(diagm([1;1;1.])))
+  
+  b_Cwp = getPPESuggested(dfg, vlbl, solveKey)
+  w_T_p = exp(M, e0, hat(M, e0, b_Cwp))
+  p_T_w = inv(M, w_T_p)
+  p_H_w = SMatrix{4,4}(affine_matrix(M, p_T_w))
+  
+  p_P1 = p_H_w * SA[w_BBo.origin...; 1.]
+  p_P2 = p_H_w * SA[(w_BBo.origin+w_BBo.widths)...; 1.]
+  
+  # pose to approximate object frame, ohat_T_p
+  # NOTE, slightly weird transform in that world rotation and local translation are mixed, so one is inverted to get consistent left action
+  # NOTE, assuming rectangular bounding box, make object frame the center of the volume
+  ohat_V_p = SA[(SA[b_Cwp[1:3]...] - (w_BBo.origin+0.5*w_BBo.widths))...]
+  ohat_T_p = ArrayPartition(ohat_V_p, SMatrix{3,3}(w_T_p.x[2]))
+  
+  (
+    GeoB.Rect3( GeoB.Point3(p_P1[1:3]...), GeoB.Point3((p_P2-p_P1)[1:3]...) ), 
+    ohat_T_p
+  )
+end
+
+"""
+    $SIGNATURES
+
+Is point p inside the HyperRectangle.
+"""
+function inside(
+  hr::GeoB.HyperRectangle, 
+  p::GeoB.Point
+)
+  #
+  _p0,_p1 = GeoB.minmax(p, hr.origin, hr.origin + hr.widths)
+  _p0 == hr.origin && hr.widths == (_p1-_p0)
+end
+
+"""
+    $SIGNATURES
+
+Create a new subcloud containing the portion of the full point cloud that is inside the mask.
+"""
+function getSubcloud(
+  pc_full::PointCloud, # the full point cloud
+  mask                 # starting out with GeometryBasics.Rect3
+)
+  objmask = map(s->_PCL.inside(mask,GeoB.Point(s.data[1:3]...)), pc_full.points)
+  spt = (s->s.data[1:3]).(pc_full.points[objmask])
+  @cast pts[i,d] := spt[i][d]
+  _PCL.PointCloud(pts)
+end
+
+
+
+#

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

@@ -193,7 +193,8 @@ function estimate_rigid_body_transformation(x_fix, y_fix, z_fix, nx_fix, ny_fix,
 
     t = x[4:6]
 
-    H = create_homogeneous_transformation_matrix(R, t)
+    H = affine_matrix(_SE3_MANI, ArrayPartition(t, R))
+    # H = create_homogeneous_transformation_matrix(R, t)
 
     return H, residuals
 
@@ -331,4 +332,17 @@ function alignICP_Simple(
 end
 
 
+function alignICP_Simple(
+  p_PC::PointCloud, 
+  hatp_PC::PointCloud;
+  kw...
+)
+  p_pts_ = (s->s.data[1:3]).(p_PC.points)
+  @cast p_pts[i,d] := p_pts_[i][d]
+  hatp_pts_ = (s->s.data[1:3]).(hatp_PC.points)
+  @cast hatp_pts[i,d] := hatp_pts_[i][d]
+
+  alignICP_Simple(p_pts, hatp_pts; kw...)
+end
+
 ##

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

@@ -408,6 +408,7 @@ Base.show(io::IO, ::MIME"application/prs.juno.inline", pc::Header) = show(io, pc
 
 
 function Base.show(io::IO, pc::PCLPointCloud2)
+  println(io)
   printstyled(io, "Caesar._PCL.PCLPointCloud2", bold=true, color=:blue)
   # println(io)
   # printstyled(io, "    T = ", bold=true, color=:magenta)
@@ -441,6 +442,7 @@ Base.show(io::IO, ::MIME"application/prs.juno.inline", pc::PCLPointCloud2) = sho
 
 
 function Base.show(io::IO, pc::PointCloud{T,P,R}) where {T,P,R}
+  println(io)
   printstyled(io, "Caesar._PCL.PointCloud{", bold=true, color=:blue)
   println(io)
   printstyled(io, "    T = ", bold=true, color=:magenta)