Merge pull request #931 from JuliaRobotics/22Q4/fix/boundingbox

obj bounding box improvements

src/3rdParty/_PCL/_PCL.jl

@@ -7,6 +7,7 @@ module _PCL
 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
+import ..Caesar: getBelief, mean
 
 using Colors
 using Dates
@@ -39,10 +40,12 @@ import IncrementalInference: ArrayPartition
 # export PointT, PointXYZ, PointXYZRGB, PointXYZRGBA
 # export inside, getSubcloud
 # export PCLHeader, PointCloud
-
+# export AbstractBoundingBox, AxisAlignedBoundingBox, OrientedBoundingBox
+# export getCorners
 
 # bring in the types
 include("entities/PCLTypes.jl")
+include("entities/OtherTypes.jl")
 # bring in further source code
 include("services/GeomBasicsUtils.jl")
 include("services/PointCloud.jl")

src/3rdParty/_PCL/entities/OtherTypes.jl

@@ -0,0 +1,70 @@
+
+abstract type AbstractBoundingBox end
+
+Base.@kwdef struct AxisAlignedBoundingBox <: AbstractBoundingBox
+  """ axis aligned bounding box, leveraging GeometryBasics.Rect3 (towards GoeB.HyperRectangle) """
+  hr::GeoB.Rect3{<:Any} = GeoB.Rect3(GeoB.Point(0,0,0.), GeoB.Point(1,1,1.))
+end
+
+function AxisAlignedBoundingBox(
+  org::Union{<:AbstractVector{<:Real}, <:GeoB.Point},
+  wdt::Union{<:AbstractVector{<:Real}, <:GeoB.Point},
+)
+  AxisAlignedBoundingBox( 
+    hr = GeoB.Rect3(
+      GeoB.Point(org...), 
+      GeoB.Point(wdt...)
+    )
+  )
+end
+
+function Base.getproperty(aabb::AxisAlignedBoundingBox, f::Symbol)
+  if f == :hr
+    getfield(aabb, f)
+  elseif f == :origin
+    aabb.hr.origin
+  elseif f == :widths
+    aabb.hr.widths
+  else
+    error("AxisAlignedBoundingBox has no field $f")
+  end
+end
+
+
+Base.@kwdef struct OrientedBoundingBox <: AbstractBoundingBox
+  """ axis aligned bounding box, leveraging GeometryBasics.Rect3 (towards GoeB.HyperRectangle) """
+  hr::GeoB.Rect3{<:Any} = GeoB.Rect3(GeoB.Point(0,0,0.), GeoB.Point(1,1,1.))
+  """ Inverse homography which goes from some reference frame r to the bounding box frame """
+  bb_H_r::typeof(SMatrix{4,4}(diagm(ones(4)))) = SMatrix{4,4}(diagm(ones(4)))
+end
+
+function OrientedBoundingBox(
+  org::Union{<:AbstractVector{<:Real}, <:GeoB.Point},
+  wdt::Union{<:AbstractVector{<:Real}, <:GeoB.Point},
+  position::Union{<:AbstractVector{<:Real}, <:GeoB.Point},
+  rotation::AbstractMatrix{<:Real}
+)
+  r_H_bb = affine_matrix(SpecialEuclidean(3), ArrayPartition(position, rotation))
+  OrientedBoundingBox(;
+    hr     = GeoB.Rect3(GeoB.Point(org...), GeoB.Point(wdt...)),
+    bb_H_r = inv(SMatrix{4,4}(r_H_bb))
+  )
+end
+
+function Base.getproperty(obb::OrientedBoundingBox, f::Symbol)
+  if f == :hr
+    getfield(obb, f)
+  elseif f == :bb_H_r
+    getfield(obb, f)
+  elseif f == :origin
+    obb.hr.origin
+  elseif f == :widths
+    obb.hr.widths
+  elseif f == :position
+    inv(obb.bb_H_r)[1:end-1, end]
+  elseif f == :rotation
+    inv(obb.bb_H_r)[1:end-1,1:end-1]
+  else
+    error("OrientedBoundingBox has no field $f")
+  end
+end

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

@@ -98,6 +98,79 @@ function Manifolds.apply(
   return _pc
 end
 
+function Manifolds.apply(
+  M::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
+  a_T_r::ArrayPartition,
+  r_BB::AbstractBoundingBox 
+)
+
+  _Hdim(::typeof(SpecialEuclidean(2))) = 3
+  _Hdim(::typeof(SpecialEuclidean(3))) = 4
+  _hdim = _Hdim(M)
+  # get the transform from obj bounding box to some reference frame r
+  _r_H_bb(::AxisAlignedBoundingBox) = SMatrix{_hdim,_hdim}(diagm(ones(_hdim)))
+  _r_H_bb(_r_BB::OrientedBoundingBox) = inv(_r_BB.bb_H_r)
+
+  a_H_bb = affine_matrix(M,a_T_r)*_r_H_bb(r_BB)
+  # ohat_H_p = inv(p_H_bb)
+  a_T_bb = ArrayPartition(a_H_bb[1:end-1,end], a_H_bb[1:end-1,1:end-1])
+
+  OrientedBoundingBox(r_BB.origin, r_BB.widths, a_T_bb.x[1], a_T_bb.x[2])
+end
+
+
+# return the bounding box p_BBo
+function transformFromWorldToLocal(
+  dfg::AbstractDFG,
+  vlbl::Symbol,
+  w_BBo::_PCL.AbstractBoundingBox;
+  solveKey=:default
+)
+  #  
+
+  v = getVariable(dfg, vlbl)
+  M = getManifold(v)
+
+  # w_T_p58 = getBelief(sfg_, :x58, :parametric) |> mean
+  # p58_T_w = inv(M, w_T_p58)
+  # p58_BBo_02 = _PCL.apply( M, p58_T_w, w_BBo_02 )
+
+  # TODO consider and consolidate with getPPESuggested or full belief
+  w_T_p = getBelief(dfg, vlbl, solveKey) |> mean
+    # 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_T_w = inv(M, w_T_p)
+  p_T_w = ArrayPartition(SA[_p_T_w.x[1]...], SMatrix{size(_p_T_w.x[2])...}(_p_T_w.x[2]))
+
+  (
+    apply( M, p_T_w, w_BBo ),
+    p_T_w
+  )
+end
+
+  # _bb_H_r(::AxisAlignedBoundingBox) = SMatrix{4,4}(diagm(ones(4)))
+  # _bb_H_r(::OrientedBoundingBox) = w_BBo.bb_H_r
+
+  # p_P1 = p_H_w * SA[w_BBo.origin...; 1.]
+  # p_P2 = p_H_w * SA[(w_BBo.origin+w_BBo.widths)...; 1.]
+
+  # p_H_bb = p_H_w*inv(_bb_H_r(w_BBo))
+  # ohat_H_p = inv(p_H_bb)
+  # ohat_T_p = ArrayPartition(ohat_H_p[end,1:end-1], ohat_H_p[1:end-1,1:end-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[w_T_p.x[1]...] - (w_BBo.origin+0.5*w_BBo.widths))...] # b_Cwp[1:3]...
+  # ohat_T_p = ArrayPartition(ohat_V_p, SMatrix{3,3}(w_T_p.x[2]))
+
+  # (
+  #   OrientedBoundingBox( w_BBo.origin, w_BBo.widths, ohat_T_p.x[1], ohat_T_p.x[2] ) # p_P1[1:3], (p_P2-p_P1)[1:3] ), 
+  #   ohat_T_p
+  # )
+
 ## ============================================================
 ## FIXME, not-yet-consolidated rigid transform code that must be deprecated below
 ## ============================================================

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

@@ -1,36 +1,37 @@
 # Utils based on GeometryBasics
 
-# 
-function transformFromWorldToLocal(
-  dfg::AbstractDFG,
-  vlbl::Symbol,
-  w_BBo::GeoB.HyperRectangle;
-  solveKey=:default
+
+"""
+    $SIGNATURES
+
+Get the eight corners of bounding box in it's local coordinates.
+
+See also: [`plotBoundingBox`](@ref)
+"""
+function getCorners(
+  BB::_PCL.AbstractBoundingBox
 )
-  #  
-  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
-  )
+  # https://math.stackexchange.com/questions/1472049/check-if-a-point-is-inside-a-rectangular-shaped-area-3d
+  hr = BB.hr
+  x_, y_, z_ = hr.origin[1], hr.origin[2], hr.origin[3]
+  dx, dy, dz = hr.widths[1], hr.widths[2], hr.widths[3]
+
+  p1 = [x_;y_;z_]
+  p2 = [x_;y_+dy;z_]
+  p4 = [x_+dx;y_;z_]
+  p3 = [x_+dx;y_+dy;z_]
+
+  p5 = [x_;y_;z_+dz]
+  p6 = [x_;y_+dy;z_+dz]
+  p8 = [x_+dx;y_;z_+dz]
+  p7 = [x_+dx;y_+dy;z_+dz]
+
+  corners_ = [p1,p2,p3,p4,p5,p6,p7,p8]
+
+  _r_H_bb(::_PCL.AxisAlignedBoundingBox) = SMatrix{4,4}(diagm(ones(4)))
+  _r_H_bb(obb::_PCL.OrientedBoundingBox) = inv(obb.bb_H_r)
+  r_H_bb = _r_H_bb(BB)
+  (s->(r_H_bb*[s...;1.])[1:3]).(corners_)
 end
 
 """
@@ -40,23 +41,46 @@ Is point p inside the HyperRectangle.
 """
 function inside(
   hr::GeoB.HyperRectangle, 
-  p::GeoB.Point
+  p #::GeoB.Point
 )
   #
-  _p0,_p1 = GeoB.minmax(p, hr.origin, hr.origin + hr.widths)
+  _p0,_p1 = GeoB.minmax(GeoB.Point(p...), hr.origin, hr.origin + hr.widths)
   _p0 == hr.origin && hr.widths == (_p1-_p0)
 end
 
+# inside(
+#   aabb::AxisAlignedBoundingBox, 
+#   p
+# ) = inside(aabb.hr, p)
+
+
+function inside(
+  obb::AbstractBoundingBox, 
+  pt
+)
+# https://math.stackexchange.com/questions/1472049/check-if-a-point-is-inside-a-rectangular-shaped-area-3d
+  p = getCorners(obb)
+
+  _i = p[2] - p[1]
+  _j = p[4] - p[1]
+  _k = p[5] - p[1]
+  _v = pt   - p[1]
+
+  # check if inside cuboid
+  (0 < _v'*_i < _i'*_i) && (0 < _v'*_j < _j'_j) && (0 < _v'*_k < _k'*_k)
+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
+  pc_full::PointCloud,       # the full point cloud
+  mask::AbstractBoundingBox  # starting out with GeometryBasics.Rect3
 )
-  objmask = map(s->_PCL.inside(mask,GeoB.Point(s.data[1:3]...)), pc_full.points)
+  objmask = map(s->_PCL.inside(mask, 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)

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

@@ -1,5 +1,6 @@
 
 export getDataPointCloud, getPointCloud, getPointCloud2D, getPointCloud3D
+export findObjectVariablesFromWorld, previewObjectSubcloudInLocalFromWorld
 
 
 function getDataPointCloud(
@@ -35,9 +36,10 @@ function getPointCloud(
   label,
   pattern = r"PCLPointCloud2", 
   w...;
+  checkhash::Bool=true,
   kw...
 )
-  pc2_a = getDataPointCloud(nfg, label, pattern)
+  pc2_a = getDataPointCloud(nfg, label, pattern; checkhash)
   if pc2_a isa Nothing
     return nothing
   end
@@ -86,7 +88,7 @@ local `p` frame coordinates (same as body frame if no lever-arm to sensor is req
 function getDataSubcloudLocal(
   dfg::AbstractDFG,
   vlb::Symbol,
-  p_BBo::GeoB.Rect3,
+  p_BBo::_PCL.AbstractBoundingBox, # GeoB.Rect3,
   bllb::Union{Symbol, UUID, <:AbstractString, Regex} = r"PCLPointCloud2";
   checkhash::Bool=true,
   pointcloud = getDataPointCloud(dfg, vlb, bllb; checkhash) |> PointCloud
@@ -105,7 +107,7 @@ subclouds for nearly correct solutions.
 function getDataSubcloudLocalFromWorld(
   dfg::AbstractDFG,
   vlb::Symbol,
-  w_BBo::GeoB.Rect3,
+  w_BBo::_PCL.AbstractBoundingBox, # GeoB.Rect3,
   bllb::Union{Symbol, UUID, <:AbstractString, Regex} = r"PCLPointCloud2";
   solveKey::Symbol = :default,
   checkhash::Bool=true
@@ -218,4 +220,62 @@ function alignPointCloudsLOOIters!(
   o_Ts_l_
 end
 
+"""
+    $SIGNATURES
+
+Find in the specified list of variables, which point clouds have at least `minpoint`s
+within the bounding box.
+
+Notes
+- Uses Caesar.jl functionality
+"""
+function findObjectVariablesFromWorld(
+  dfg::AbstractDFG,
+  r_BBo::_PCL.AbstractBoundingBox;
+  solveKey::Symbol = :default,
+  minpoints::Int=5000,
+  limit::Int=50,
+  varPattern::Regex=r"x\d+",
+  tags::AbstractVector{Symbol}=Symbol[],
+  varList::AbstractVector{Symbol} = sort(ls(dfg,varPattern;tags); lt=DFG.natural_lt),
+  blobLabel = r"PCLPointCloud2"
+)
+  M = SpecialEuclidean(3)
+  objVars = Symbol[]
+
+  for vlb in varList
+    b_PC = _PCL.getDataPointCloud(dfg, vlb, blobLabel; checkhash=false) |> _PCL.PointCloud
+    # TODO use PPE after PPEs are updated to Manifolds representation
+    r_T_b = getBelief(dfg, vlb, solveKey) |> mean
+    r_PC = _PCL.apply(M, r_T_b, b_PC)
+    sc = _PCL.getSubcloud(r_PC, r_BBo)
+    if minpoints < length(sc)
+      push!(objVars, vlb)
+    end
+  end
+
+  len = length(objVars)
+  if 0<len
+    idx = unique(Int.(round.(1:len/limit:len)))
+    objVars[idx]
+  else
+    objVars
+  end
+end
+
+#
+
+function previewObjectSubcloudInLocalFromWorld(
+  dfg::AbstractDFG,
+  vlb::Symbol,
+  w_BBo::AbstractBoundingBox;
+  solveKey::Symbol = :default,
+  blobLabel = r"PCLPointCloud2",
+  checkhash::Bool=true
+)
+  p_PC = getDataPointCloud(dfg, vlb, blobLabel; checkhash) |> _PCL.PointCloud  
+  p_BBo, o_T_p = transformFromWorldToLocal(dfg, vlb, w_BBo; solveKey)
+  getSubcloud(p_PC, p_BBo)
+end
+
 #