Merge pull request #942 from JuliaRobotics/23Q1/enh/objclosures

allow loopObject on OAS factor for closures

src/3rdParty/_PCL/_PCL.jl

@@ -44,6 +44,7 @@ import IncrementalInference: ArrayPartition
 # export PCLHeader, PointCloud
 # export AbstractBoundingBox, AxisAlignedBoundingBox, OrientedBoundingBox
 # export getCorners
+# export exportPointCloudWorld
 
 # bring in the types
 include("entities/PCLTypes.jl")

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

@@ -216,9 +216,9 @@ function PointCloud(
 end
 
 function PointCloud(
-    xyz::AbstractMatrix{<:Real};
-    kwargs...
-  )
+  xyz::AbstractMatrix{<:Real};
+  kwargs...
+)
   #
   PointCloud(
     view(xyz,:,1),
@@ -228,6 +228,17 @@ function PointCloud(
   )
 end
 
+function PointCloud(
+  xyz::AbstractVector{<:AbstractVector{<:Real}};
+  kwargs...
+)
+  @cast mat[i,d] := xyz[i][d]
+  PointCloud(
+    mat;
+    kwargs...
+  )
+end
+
 
 # https://pointclouds.org/documentation/conversions_8h_source.html#l00166
 function PointCloud(

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

@@ -303,7 +303,7 @@ function findObjectVariablesFromWorld(
   minList::Int = 1,
   maxList::Int = 999999,
   minrange=1.0,
-  varList::AbstractVector{Symbol} = sort(ls(dfg,varPattern;tags); lt=DFG.natural_lt)[minList:maxList],
+  varList::AbstractVector{Symbol} = sort(ls(dfg,varPattern;tags); lt=DFG.natural_lt)[minList:minimum([end;maxList])],
   blobLabel = r"PCLPointCloud2",
   sortList::Bool=true,
   checkhash::Bool=false
@@ -433,7 +433,12 @@ function calcAxes3D(
       end
     end
     # costs = [distance(Mr, R0, R); distance(Mr, R0, Rx); distance(Mr, R0, Ry)]
-    arr[argmin(costs)]
+    if 0 < length(costs)
+      arr[argmin(costs)]
+    else
+      @warn "not able to approximate an orientation for the new object, using default idenity"
+      R0
+    end
   end
 
   ArrayPartition(SVector(μ...), SMatrix{3,3}(R_enh))
@@ -464,4 +469,52 @@ function Base.convert(
   PointCloud(xyz)
 end
 
+"""
+    $SIGNATURES
+
+Return a PointCloud with all the points in the world frame coordinates, given the solveKey.
+
+See also: [`saveLAS`](@ref)
+"""
+function exportPointCloudWorld(
+  dfg::AbstractDFG;
+  varList::AbstractVector{Symbol} = sort(ls(dfg); lt=DFG.natural_lt),
+  solveKey::Symbol = :default,
+  # TODO update to blobs saved as LAS files instead
+  getpointcloud::Function = (v)->_PCL.getDataPointCloud(dfg, v, Regex("PCLPointCloud2"); checkhash=false),
+  downsample::Int=1,
+  minrange = 0.0,
+  maxrange = 9999.0,
+)
+  M = SpecialEuclidean(3)
+  ϵ0 = ArrayPartition(SVector(0,0,0.),SMatrix{3,3}(1,0,0,0,1,0,0,0,1.)) # MJL.identity_element(M)
+  pc_map = _PCL.PointCloud()
+  # loop through all variables in the given list
+  for vl in varList
+    pc_ = getpointcloud(vl)
+    if pc_ isa Nothing
+      @warn "Skipping variable without point cloud" vl
+      continue
+    end
+    pc = PointCloud(pc_)
+
+    pts_a = (s->[s.x;s.y;s.z]).(pc.points)
+    pts_a = _filterMinRange(pts_a, minrange, maxrange)
+    pc = PointCloud(pts_a)
+
+    v = getVariable(dfg, Symbol(vl))
+    if !(:parametric in listSolveKeys(v))
+      @warn "Skipping $vl which does not have solveKey :parametric"
+      continue
+    end
+    w_Cwp = calcPPE(v; solveKey).suggested
+    wPp = Manifolds.exp(M,ϵ0,Manifolds.hat(M,ϵ0,w_Cwp))
+    # wPp = getSolverData(v, solveKey).val[1]
+    wPC = apply(M, wPp, pc)
+    cat(pc_map, wPC; reuse=true)
+  end
+
+  pc_map
+end
+
 #

src/objects/ObjectAffordanceSubcloud.jl

@@ -75,6 +75,21 @@ Base.@kwdef struct ObjectAffordanceSubcloud <: AbstractManifoldMinimize
   added to the same object variable later by the user.  See [`IIF.rebuildFactorMetadata!`](@ref).
   FIXME: there is a hack, should not be using Serialization.deserialize on a PCLPointCloud2, see Caesar.jl#921 """
   p_PC_blobIds::Vector{UUID} = Vector{UUID}()
+  """
+  Object Affordance factor can be used to "close loops" if the user knows two object are the same.
+  E.g. say two OAS factors already make two :OBJECT_AFFORDANCE variables of the same physical object,
+  then creating a third and loop obj variable and third OAS factor with .loopObject=true against the 
+  existing two object variables should effectively "close the loop".
+  """
+  loopObject::Bool = false
+  """
+  Alignment parameters
+  """
+  alignParams::Dict{Symbol,Int} = Dict{Symbol,Int}(  
+                                    :max_iterations => 40,
+                                    :correspondences => 500,
+                                    :neighbors => 50
+                                  )
 end
 
 # function Base.getproperty(oas::ObjectAffordanceSubcloud, f::Symbol)
@@ -127,6 +142,18 @@ function _findObjPriors(dfg::AbstractDFG, fvars::AbstractVector{<:DFGVariable})
   return objpriors, op_PCs
 end
 
+function _findObjAffSubcFactor(dfg::AbstractDFG, objl::Symbol)
+  # find oas factor label where objl is first variable
+  fcts = ls(dfg, objl)
+  # loop through all factors on this obj variable
+  for fc in getFactor.(dfg, fcts)
+    vo = getVariableOrder(fc)
+    # check that it is the OAS factor defining this vo[1] obj Variable 
+    if 1 < length(vo) && vo[1] == objl 
+      return getLabel(fc)
+    end
+  end
+end
 
 function _defaultOASCache(
   dfg::AbstractDFG, 
@@ -160,13 +187,26 @@ function _defaultOASCache(
 
   # NOTE, obj variable first, pose variables are [2:end]
   for (i,vl) in enumerate(getLabel.(fvars)[2:end])
-    p_PC = _PCL.getDataPointCloud(dfg, vl, fct.p_PC_blobIds[i]; checkhash=false) |> _PCL.PointCloud
-    p_SC = _PCL.getSubcloud(p_PC, fct.p_BBos[i]; minrange, maxrange)
+    lhat_T_p_, p_SC = if !fct.loopObject
+      p_PC = _PCL.getDataPointCloud(dfg, vl, fct.p_PC_blobIds[i]; checkhash=false) |> _PCL.PointCloud
+      p_SC_ = _PCL.getSubcloud(p_PC, fct.p_BBos[i]; minrange, maxrange)
+      fct.lhat_Ts_p[i], p_SC_
+    else
+      # use this when OAS is used to close loop on already existing ObjAff variables
+      @assert :OBJECT_AFFORDANCE in getTags(fvars[i]) "ObjAffSubc factor with .loopObject = true must connect to variables with the tag :OBJECT_AFFORDANCE"
+      foaslb = _findObjAffSubcFactor(dfg, vl)
+      # get the obj aff point cloud
+      o_SC_ = Caesar.assembleObjectCache(dfg, foaslb)
+      # transform obj aff clouds to same reference frame
+      w_T_o_ = fct.lhat_Ts_p[i] |> deepcopy # getBelief(dfg, vl) |> mean
+      # return transform to common frame and object frame subcloud
+      w_T_o_, o_SC_
+    end
+    # sanity check
     if 0 === length(p_SC)
       error("ObjectAffordance factor cannot use empty subcloud on $(vl)")
     end
-
-    lhat_T_p_ = fct.lhat_Ts_p[i]
+    # use mutable format
     lhat_T_p = ArrayPartition(
       MVector(lhat_T_p_.x[1]...), 
       MMatrix{size(lhat_T_p_.x[2])...}(lhat_T_p_.x[2])
@@ -229,6 +269,14 @@ function IncrementalInference.preambleCache(
   for i in 1:length(cache.ohat_SCs)
     push!(cache.o_Ts_ohat, e0)
   end
+  k_ = keys(fct.alignParams) |> collect
+  v_ = values(fct.alignParams) |> collect
+  arr = []
+  for i in 1:length(k_)
+    push!(arr, (k_[i]=>v_[i]))
+  end
+  tup = (arr...,)
+  lookws = (;tup...)
 
   # finalize object point clouds for cache
   # align if there if there is at least one LIE transform and cloud available.
@@ -237,7 +285,8 @@ function IncrementalInference.preambleCache(
     _PCL.alignPointCloudsLOOIters!(
       cache.o_Ts_ohat, 
       cache.ohat_SCs, 
-      true
+      true;
+      lookws...   
     )
   end
 
@@ -361,8 +410,18 @@ function generateObjectAffordanceFromWorld!(
   w_BBobj::_PCL.AbstractBoundingBox;
   solveKey::Symbol = :default,
   pcBlobLabel = r"PCLPointCloud2",
-  modelprior::Union{Nothing,<:_PCL.PointCloud}=nothing
+  modelprior::Union{Nothing,<:_PCL.PointCloud}=nothing,
+  loopObject::Bool = false,
+  alignParams::Dict{Symbol,Int} = Dict{Symbol,Int}(  
+                                    :max_iterations => 40,
+                                    :correspondences => 500,
+                                    :neighbors => 50
+                                  )
 )
+  if 0 === length(vlbs)
+    @error "No variables from which to build and object affordance."
+    return nothing
+  end
   M = SpecialEuclidean(3) # getManifold(Pose3)
   # add the object variable
   addVariable!(dfg, olb, Pose3; tags=[:OBJECT_AFFORDANCE])
@@ -374,24 +433,34 @@ function generateObjectAffordanceFromWorld!(
   end
 
   # add the object affordance subcloud factors
-  oas = ObjectAffordanceSubcloud()
+  oas = ObjectAffordanceSubcloud(;loopObject, alignParams)
 
   for vlb in vlbs
     # make sure PPE is set on this solveKey
     setPPE!(dfg, vlb, solveKey)
-    # lhat frame is some local frame where object subclouds roughly fall together (could be host start from world frame)
-    p_BBo, p_T_lhat = _PCL.transformFromWorldToLocal(dfg, vlb, w_BBobj; solveKey)
-    lhat_T_p_ = inv(M, p_T_lhat)
-    # make immutable data type
-    lhat_T_p = ArrayPartition(SA[lhat_T_p_.x[1]...], SMatrix{3,3}(lhat_T_p_.x[2]))
-    p_PC_blobId = getDataEntry(dfg, vlb, pcBlobLabel).id
-    push!(oas.p_BBos, p_BBo)
-    push!(oas.lhat_Ts_p, lhat_T_p)
-    push!(oas.p_PC_blobIds, p_PC_blobId)
+    if !loopObject
+      # lhat frame is some local frame where object subclouds roughly fall together (could be host start from world frame)
+      p_BBo, p_T_lhat = _PCL.transformFromWorldToLocal(dfg, vlb, w_BBobj; solveKey)
+      lhat_T_p_ = inv(M, p_T_lhat)
+      # make immutable data type
+      lhat_T_p = ArrayPartition(SA[lhat_T_p_.x[1]...], SMatrix{3,3}(lhat_T_p_.x[2]))
+      p_PC_blobId = getDataEntry(dfg, vlb, pcBlobLabel).id
+      push!(oas.p_BBos, p_BBo)
+      push!(oas.lhat_Ts_p, lhat_T_p)
+      push!(oas.p_PC_blobIds, p_PC_blobId)
+    else
+      w_T_o = getBelief(dfg, vlb, solveKey) |> mean
+      push!(oas.lhat_Ts_p, w_T_o) # inv(M, w_T_o)) # do not understand the inverse?
+    end
   end
 
-  addFactor!(dfg, [olb; vlbs], oas)
-
+  try
+    addFactor!(dfg, [olb; vlbs], oas)
+  catch err
+    deleteVariable!(dfg, olb)
+    rethrow(err)
+  end
+
   oas
 end
 
@@ -517,7 +586,8 @@ function protoObjectCheck!(
   minrange = 0.75,
   varList::AbstractVector{Symbol}=_PCL.findObjectVariablesFromWorld(dfg, w_BBo; solveKey, limit, minpoints, selection, minList, maxList),
   obl::Symbol = :testobj,
-  align::Symbol = :fine
+  align::Symbol = :fine,
+  oaskw...
 )
   #
   try
@@ -529,7 +599,8 @@ function protoObjectCheck!(
       obl,
       varList,
       w_BBo;
-      solveKey
+      solveKey,
+      oaskw...
     )
 
     flb = intersect((ls.(dfg, varList))..., ls(dfg, obl))[1]