From 9666cd44076a3bd1c83f997902c0d44bd6205901 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 1 Feb 2023 01:09:03 -0800 Subject: [PATCH 1/6] allow loopObject on OAS factor for closures --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 2 +- src/objects/ObjectAffordanceSubcloud.jl | 74 ++++++++++++++----- 2 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index e745df40a..825a4a86c 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/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 diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index e85c1859b..bccd530c3 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -75,6 +75,13 @@ 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 end # function Base.getproperty(oas::ObjectAffordanceSubcloud, f::Symbol) @@ -127,6 +134,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 +179,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]) @@ -361,7 +393,8 @@ 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 ) M = SpecialEuclidean(3) # getManifold(Pose3) # add the object variable @@ -374,20 +407,25 @@ function generateObjectAffordanceFromWorld!( end # add the object affordance subcloud factors - oas = ObjectAffordanceSubcloud() + oas = ObjectAffordanceSubcloud(;loopObject) 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) @@ -517,7 +555,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, + loopObject::Bool=false, ) # try @@ -529,7 +568,8 @@ function protoObjectCheck!( obl, varList, w_BBo; - solveKey + solveKey, + loopObject ) flb = intersect((ls.(dfg, varList))..., ls(dfg, obl))[1] From b4b6dc378b5a0718d7e168b21db6c28b80b92dd1 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 2 Feb 2023 23:10:43 -0800 Subject: [PATCH 2/6] align params for obj closure --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 7 ++- src/objects/ObjectAffordanceSubcloud.jl | 45 ++++++++++++++++--- 2 files changed, 44 insertions(+), 8 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 825a4a86c..bba783f65 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -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)) diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index bccd530c3..d7a0f64c3 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -82,6 +82,14 @@ Base.@kwdef struct ObjectAffordanceSubcloud <: AbstractManifoldMinimize 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) @@ -261,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. @@ -269,7 +285,8 @@ function IncrementalInference.preambleCache( _PCL.alignPointCloudsLOOIters!( cache.o_Ts_ohat, cache.ohat_SCs, - true + true; + lookws... ) end @@ -394,8 +411,17 @@ function generateObjectAffordanceFromWorld!( solveKey::Symbol = :default, pcBlobLabel = r"PCLPointCloud2", modelprior::Union{Nothing,<:_PCL.PointCloud}=nothing, - loopObject::Bool = false + 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]) @@ -407,7 +433,7 @@ function generateObjectAffordanceFromWorld!( end # add the object affordance subcloud factors - oas = ObjectAffordanceSubcloud(;loopObject) + oas = ObjectAffordanceSubcloud(;loopObject, alignParams) for vlb in vlbs # make sure PPE is set on this solveKey @@ -428,8 +454,13 @@ function generateObjectAffordanceFromWorld!( end end - addFactor!(dfg, [olb; vlbs], oas) - + try + addFactor!(dfg, [olb; vlbs], oas) + catch err + deleteVariable!(dfg, olb) + rethrow(err) + end + oas end @@ -556,7 +587,7 @@ function protoObjectCheck!( varList::AbstractVector{Symbol}=_PCL.findObjectVariablesFromWorld(dfg, w_BBo; solveKey, limit, minpoints, selection, minList, maxList), obl::Symbol = :testobj, align::Symbol = :fine, - loopObject::Bool=false, + oaskw... ) # try @@ -569,7 +600,7 @@ function protoObjectCheck!( varList, w_BBo; solveKey, - loopObject + oaskw... ) flb = intersect((ls.(dfg, varList))..., ls(dfg, obl))[1] From a7e7c43a97414b62a3e0104b38c5e7a16ba504bf Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 2 Feb 2023 23:27:57 -0800 Subject: [PATCH 3/6] export world map and constr PointCloud utils --- src/3rdParty/_PCL/services/PointCloud.jl | 17 ++++--- src/3rdParty/_PCL/services/PointCloudUtils.jl | 47 +++++++++++++++++++ 2 files changed, 58 insertions(+), 6 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl index 104ef53ec..fa1f7db9e 100644 --- a/src/3rdParty/_PCL/services/PointCloud.jl +++ b/src/3rdParty/_PCL/services/PointCloud.jl @@ -216,18 +216,23 @@ function PointCloud( end function PointCloud( - xyz::AbstractMatrix{<:Real}; - kwargs... - ) + xyz::AbstractMatrix{<:Real}; + kwargs... +) # + @cast mat[i,d] := xyz[i][d] PointCloud( - view(xyz,:,1), - view(xyz,:,2), - view(xyz,:,3); + mat; kwargs... ) end +function PointCloud( + xyz::AbstractVector{<:AbstractVector{<:Real}}; + kwargs... +) + + # https://pointclouds.org/documentation/conversions_8h_source.html#l00166 function PointCloud( diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index bba783f65..b86636956 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -469,4 +469,51 @@ 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 exportPointCloudsInWorld( + dfg::AbstractDFG; + varList::AbstractVector{Symbol} = sort(ls(dfg); lt=DFG.natural_lt), + solveKey::Symbol = :default, + 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,MJL.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 + # \ No newline at end of file From bf9778c1e0cb98827fd63e833111e7d3b6e50ad4 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 2 Feb 2023 23:30:26 -0800 Subject: [PATCH 4/6] better naming --- src/3rdParty/_PCL/_PCL.jl | 1 + src/3rdParty/_PCL/services/PointCloudUtils.jl | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/3rdParty/_PCL/_PCL.jl b/src/3rdParty/_PCL/_PCL.jl index db630f109..2fdf4f0a0 100644 --- a/src/3rdParty/_PCL/_PCL.jl +++ b/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") diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index b86636956..a4aebf31c 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -476,7 +476,7 @@ Return a PointCloud with all the points in the world frame coordinates, given th See also: [`saveLAS`](@ref) """ -function exportPointCloudsInWorld( +function exportPointCloudWorld( dfg::AbstractDFG; varList::AbstractVector{Symbol} = sort(ls(dfg); lt=DFG.natural_lt), solveKey::Symbol = :default, @@ -507,7 +507,7 @@ function exportPointCloudsInWorld( continue end w_Cwp = calcPPE(v; solveKey).suggested - wPp = Manifolds.exp(M,ϵ0,MJL.hat(M,ϵ0,w_Cwp)) + 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) From 5490a0734453bef863a8c916df02a89c9aea5e56 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 2 Feb 2023 23:32:16 -0800 Subject: [PATCH 5/6] oops --- src/3rdParty/_PCL/services/PointCloud.jl | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl index fa1f7db9e..6a7801097 100644 --- a/src/3rdParty/_PCL/services/PointCloud.jl +++ b/src/3rdParty/_PCL/services/PointCloud.jl @@ -220,9 +220,10 @@ function PointCloud( kwargs... ) # - @cast mat[i,d] := xyz[i][d] PointCloud( - mat; + view(xyz,:,1), + view(xyz,:,2), + view(xyz,:,3); kwargs... ) end @@ -231,7 +232,12 @@ 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 From 1c1d12203fc2b294d01e7fffc04d5ca795e7fb37 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 2 Feb 2023 23:34:44 -0800 Subject: [PATCH 6/6] and another fix --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index a4aebf31c..f35ae0678 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -480,7 +480,8 @@ function exportPointCloudWorld( dfg::AbstractDFG; varList::AbstractVector{Symbol} = sort(ls(dfg); lt=DFG.natural_lt), solveKey::Symbol = :default, - getpointcloud::Function = (v)->_PCL.getDataPointCloud(dfg, v, Regex("PCLPointCloud2"); checkhash=false); + # 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,