From 0e57c1559d77245627e4d4c2a1f7defc7e7e34f9 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 2 Jan 2023 18:23:27 -0800 Subject: [PATCH 01/12] adding obj model priors (MetaPrior) --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 11 ++- src/objects/ObjectAffordanceSubcloud.jl | 82 +++++++++---------- 2 files changed, 49 insertions(+), 44 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 77f1c6732..16c3bcc7b 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -219,16 +219,21 @@ end Use sequential leave-one-out iteration strategy to self align assuming the list of point clouds have good coverage of the same object. + +Notes +- Leave-one-out on the first iterLength number of elements. + - Manually set `iterLength` to skip the tail elements of list -- e.g. skipping model priors. """ function alignPointCloudsLOOIters!( o_Ts_l::AbstractVector{<:ArrayPartition}, l_PCs::AbstractVector{<:_PCL.PointCloud}, updateTloo::Bool=true, - MC::Int=3, + MC::Int=3; + iterLength::Int=length(l_PCs) ) o_Ts_l_ = (updateTloo ? s->s : deepcopy)(o_Ts_l) - - for m in 1:MC, k in 1:length(l_PCs) + + for m in 1:MC, k in 1:iterLength o_Tloo, o_PClie, o_PCloo = alignPointCloudLOO!( o_Ts_l_, l_PCs, diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index 879ff4fcf..e201d8430 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -86,42 +86,30 @@ end getManifold(oas::ObjectAffordanceSubcloud) = PowerManifold(getManifold(Pose3Pose3), NestedReplacingPowerRepresentation(), length(oas.ohat_Ts_p)) -# function _findOASPriors() -# # get all pose variables connected to this object -# loovlb = getLabel(fvars[1]) -# olbl = getLabel(fvars[2]) -# # will not yet include this new factor. -# aflb = ls(dfg,olbl) -# neis = ls.(dfg,aflb) -# avlbs = 0 < length(neis) ? union(neis...) : neis -# avlbs = setdiff(avlbs, [getLabel(fvars[2]);]) -# # should make sure loovlb removed, since `IIF.rebuildFactorMetadata!` usage affects the lie loo caching steps -# lievlbs = setdiff(avlbs, [loovlb;]) -# fc_lie_lbls = Symbol[] - -# # define the LIE elements -# for liev in lievlbs -# neifl = intersect(aflb,ls(dfg,liev)) -# # sibling factors -# neifc = getFactorType.(dfg,neifl) -# # must be OAS factor -# filter!(f->f isa ObjectAffordanceSubcloud, neifc) -# # filtering from all factors, skip this variable if not part of this object affordance -# 0 === length(neifl) ? continue : nothing -# nfclb = neifl[1] -# nfc = neifc[1] -# # collect factor labels -# push!(fc_lie_lbls, nfclb) - -# # collect list of all subclouds -# # p_SC = _PCL.getDataSubcloudLocal(dfg, liev, fct.p_BBo, r"PCLPointCloud2"; checkhash=false) -# # push!(p_SClie, p_SC) -# # # collect list of all pose to object transforms -# # # FIXME, should start with current best guess provided by user -# # push!(o_Tlie_p, nfc.ohat_T_p) # e0 -# end +struct ObjectModelPrior end -# end +function _findObjPriors(dfg::AbstractDFG, fvars::AbstractVector{<:DFGVariable}) + objlb = getLabel(fvars[1]) + aflbs = ls(dfg,objlb) + + objpriors = Symbol[] + + for flb in aflbs + neifc = getFactorType.(dfg,flb) + # must be OAS factor + filter!(f->f isa MetaPrior{<:ObjectModelPrior}, neifc) + # filtering from all factors, skip this variable if not part of this object affordance + 0 === length(neifc) ? continue : nothing + push!(objpriors, flb) + end + + @assert length(objpriors) < 2 "Only one MetaPrior can be added to the object variable." + if length(objpriors) == 1 + return objpriors, _PCL.getDataPointCloud(dfg, objpriors[1], r"PCLPointCloud2"; checkhash=false) + else + return objpriors, nothing + end +end function _defaultOASCache( @@ -129,6 +117,7 @@ function _defaultOASCache( fvars::AbstractVector{<:DFGVariable}, fct::ObjectAffordanceSubcloud ) + # use concrete types _PCT(::PC) where {PC <: _PCL.PointCloud} = PC PCT = _PCT(_PCL.PointCloud()) @@ -155,7 +144,16 @@ function _defaultOASCache( push!(cache.p_SCs, p_SC) push!(cache.o_Ts_p, ohat_T_p) end - + + # check and add any priors + objlbs, o_PC = _findObjPriors(dfg, fvars) + if 0 < length(objlbs) + # NOTE assume just one model prior allowed (if present) + push!(cache.p_SCs, o_PC) + push!(cache.o_Ts_p, e0) + push!(cache.objPrior, objlbs[1]) + end + return cache end @@ -177,10 +175,12 @@ function IncrementalInference.preambleCache( # finalize object point clouds for cache # align if there if there is at least one LIE transform and cloud available. if 0 < length(cache.o_Ts_p) + # manually set iterLength to skip possible priors, length(fvars) minus the object variable oo_Ts_p = _PCL.alignPointCloudsLOOIters!( cache.o_Ts_p, cache.p_SCs, - true, 3 + true, 3; + iterLength=length(cache.o_Ts_p)-length(cache.objPriors) # skip priors ) end # update the cached memory pointers @@ -209,12 +209,12 @@ function IncrementalInference.getSample( # M = getManifold(cf.factor).manifold e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(diagm(ones(3)))) - len = length(cf.cache.o_Ts_p) + iterLength = length(cf.cache.o_Ts_p)-length(cf.cache.objPriors) - Xs = Vector{typeof(e0)}(undef, len) + Xs = Vector{typeof(e0)}(undef, iterLength) - # TODO, incorporate PointCloudPriors - for i in 1:len + # NOTE cache.o_Ts_p and .p_SCs include the model priors at end of lists + for i in 1:iterLength # TBD consider adding a perturbation before alignment oo_Tloo_p, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( cf.cache.o_Ts_p, From e7074e87eeff53b9c6d109e339e4cc9a6d140b44 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 2 Jan 2023 21:22:26 -0800 Subject: [PATCH 02/12] testing with ObjectModelPrior --- src/objects/ObjectAffordanceSubcloud.jl | 54 +++++++++---------------- 1 file changed, 19 insertions(+), 35 deletions(-) diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index e201d8430..448554a78 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -86,7 +86,9 @@ end getManifold(oas::ObjectAffordanceSubcloud) = PowerManifold(getManifold(Pose3Pose3), NestedReplacingPowerRepresentation(), length(oas.ohat_Ts_p)) -struct ObjectModelPrior end +struct ObjectModelPrior + pc::_PCL.PointCloud +end function _findObjPriors(dfg::AbstractDFG, fvars::AbstractVector{<:DFGVariable}) objlb = getLabel(fvars[1]) @@ -95,12 +97,11 @@ function _findObjPriors(dfg::AbstractDFG, fvars::AbstractVector{<:DFGVariable}) objpriors = Symbol[] for flb in aflbs - neifc = getFactorType.(dfg,flb) + neifc = getFactorType(dfg,flb) # must be OAS factor - filter!(f->f isa MetaPrior{<:ObjectModelPrior}, neifc) - # filtering from all factors, skip this variable if not part of this object affordance - 0 === length(neifc) ? continue : nothing - push!(objpriors, flb) + if neifc isa MetaPrior{<:ObjectModelPrior} + push!(objpriors, flb) + end end @assert length(objpriors) < 2 "Only one MetaPrior can be added to the object variable." @@ -180,7 +181,7 @@ function IncrementalInference.preambleCache( cache.o_Ts_p, cache.p_SCs, true, 3; - iterLength=length(cache.o_Ts_p)-length(cache.objPriors) # skip priors + iterLength=length(cache.o_Ts_p)-length(cache.objPrior) # skip priors ) end # update the cached memory pointers @@ -209,7 +210,7 @@ function IncrementalInference.getSample( # M = getManifold(cf.factor).manifold e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(diagm(ones(3)))) - iterLength = length(cf.cache.o_Ts_p)-length(cf.cache.objPriors) + iterLength = length(cf.cache.o_Ts_p)-length(cf.cache.objPrior) Xs = Vector{typeof(e0)}(undef, iterLength) @@ -311,32 +312,7 @@ function makePointCloudObjectAffordance( getFactorType(fc).ohat_Ts_p end - ohat_SCobj = _PCL.mergePointCloudsWithTransforms(o_Ts_p, cache.p_SCs,) - - # M = getManifold(Pose3) - # # the final output object point cloud object (in object frame) - # ohat_SCobj = _PCL.PointCloud() - - # for flb in ls(dfg, ovl) - # # work from user and factor cache data - # fc = getFactor(dfg, flb) - # # get a factor subcloud of the object in the pose frame - # p_SC = IIF._getCCW(fc).dummyCache.p_SCloo[] - # # get the transform from pose to object frame - # o_T_p = if align == :fine - # @warn("Using preamble fine alignment from $flb") - # IIF._getCCW(fc).dummyCache.o_Tloo_p[] - # else - # getFactorType(dfg, flb).ohat_T_p - # end - # # convert the pose keyframe point cloud to - # ohat_SC = _PCL.apply(M, o_T_p, p_SC) - # cat( - # ohat_SCobj, - # ohat_SC; - # reuse = true - # ) - # end + ohat_SCobj = _PCL.mergePointCloudsWithTransforms(o_Ts_p, cache.p_SCs) return ohat_SCobj end @@ -348,11 +324,19 @@ function generateObjectAffordanceFromWorld!( vlbs::AbstractVector{<:Symbol}, w_BBobj::_PCL.AbstractBoundingBox; solveKey::Symbol = :default, - pcBlobLabel = r"PCLPointCloud2" + pcBlobLabel = r"PCLPointCloud2", + modelprior::Union{Nothing,<:_PCL.PointCloud}=nothing ) M = SpecialEuclidean(3) # getManifold(Pose3) # add the object variable addVariable!(dfg, olb, Pose3) + + # if a model prior exists, add it here + if !isnothing(modelprior) + mpr = MetaPrior(;data=ObjectModelPrior(modelprior)) + addFactor!(dfg, [olb;], mpr; tags=[:MODELPRIOR]) + end + # add the object affordance subcloud factors oas = ObjectAffordanceSubcloud() From 172cdfce794262f95e2368deaa279f6da5ab9820 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 2 Jan 2023 23:00:59 -0800 Subject: [PATCH 03/12] wip --- src/objects/ObjectAffordanceSubcloud.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index 448554a78..dd8dd136b 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -106,7 +106,7 @@ function _findObjPriors(dfg::AbstractDFG, fvars::AbstractVector{<:DFGVariable}) @assert length(objpriors) < 2 "Only one MetaPrior can be added to the object variable." if length(objpriors) == 1 - return objpriors, _PCL.getDataPointCloud(dfg, objpriors[1], r"PCLPointCloud2"; checkhash=false) + return objpriors, _PCL.getDataPointCloud(dfg, objpriors[1], r"PointCloudLAS"; checkhash=false) else return objpriors, nothing end From c4531488a01465d55e22e8b14854b337c862a218 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 12 Jan 2023 01:21:07 -0800 Subject: [PATCH 04/12] towards usable model priors --- Project.toml | 3 +- src/3rdParty/_PCL/_PCL.jl | 1 + src/3rdParty/_PCL/services/PointCloudUtils.jl | 28 ++++++ src/objects/ObjectAffordanceSubcloud.jl | 99 ++++++++++++------- 4 files changed, 95 insertions(+), 36 deletions(-) diff --git a/Project.toml b/Project.toml index ffde6956c..5498cbf9f 100644 --- a/Project.toml +++ b/Project.toml @@ -29,6 +29,7 @@ JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3" KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" +MultivariateStats = "6f286f6a-111f-5878-ab1e-185364afe411" NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" NearestNeighbors = "b8a86587-4115-5ab1-83bc-aa920d37bbce" Optim = "429524aa-4258-5aef-a3af-852621145aeb" @@ -57,7 +58,7 @@ Colors = "0.12" Combinatorics = "1" CoordinateTransformations = "0.5, 0.6" DataStructures = "0.17, 0.18" -DistributedFactorGraphs = "0.18" +DistributedFactorGraphs = "0.18, 0.19" Distributions = "0.25" DocStringExtensions = "0.8, 0.9" FFTW = "1" diff --git a/src/3rdParty/_PCL/_PCL.jl b/src/3rdParty/_PCL/_PCL.jl index 80156939e..db630f109 100644 --- a/src/3rdParty/_PCL/_PCL.jl +++ b/src/3rdParty/_PCL/_PCL.jl @@ -26,6 +26,7 @@ import GeometryBasics as GeoB # name collisions on members: Point, etc. using DistributedFactorGraphs using TensorCast using UUIDs +using MultivariateStats # FIXME REMOVE, only used for legacy getDataPointCloud using Serialization diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 16c3bcc7b..42fe0e883 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -1,6 +1,7 @@ export getDataPointCloud, getPointCloud, getPointCloud2D, getPointCloud3D export findObjectVariablesFromWorld, previewObjectSubcloudInLocalFromWorld +export calcAxes3D function getDataPointCloud( @@ -303,4 +304,31 @@ function previewObjectSubcloudInLocalFromWorld( getSubcloud(p_PC, p_BBo) end +""" + $SIGNATURES + +Calculate a new local to object (`l_T_o`) axis frame for a point cloud using PCA. + +Example +```julia +M = SpecialEuclidean(3) +l_T_o = _PCL.calcAxes3D(l_PC) + +# should return zeros when centered around the new object frame +iszeros = _PCL.calcAxes3D(_PCL.apply(M, inv(M, l_T_o), pc)) +``` +""" +function calcAxes3D( + pc::PointCloud +) + # + xyz_ = (p->[p.x;p.y;p.z]).(pc.points) + @cast xyz[d,i] := xyz_[i][d] + mdl = fit(PCA, xyz; pratio=1) + R = projection(mdl) + μ = mean(xyz; dims=2) + + ArrayPartition(SVector(μ...), SMatrix{3,3}(R)) +end + # \ No newline at end of file diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index dd8dd136b..b624e304c 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -9,10 +9,11 @@ Base.@kwdef struct _ObjAffSubcCache """ list of object subclouds in individual pose reference frames """ p_SCs::Vector{<:_PCL.PointCloud} """ pose to object transforms for each indivudal pose variable """ - o_Ts_p::Vector{<:ArrayPartition} + bb_Ts_p::Vector{<:ArrayPartition} + """ individual transforms back from each subcloud reference to a common object reference frame """ + o_Ts_bb::Vector{<:ArrayPartition} """ any object priors connected to the landmark variable """ objPrior::Vector{Symbol} = Symbol[] - # TODO add logic or necessary registers for the common object reference frame # TODO add registers for ObjectAffordancePrior end # """ extracted subclouds from connected LIE factors """ @@ -58,7 +59,7 @@ Base.@kwdef struct ObjectAffordanceSubcloud <: AbstractManifoldMinimize pose to object offset (or pose in object frame) to where the center of the object's bounding box. I.e. the user provided initial guess of relative transform to go from pose to object frame. Note, the type here is restricted to StaticArrays only, to ensure best alignments are stored in the cache. """ - ohat_Ts_p::Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))} = Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))}() + bb_Ts_p::Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))} = Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))}() """ standard entry blob label where to find the point clouds -- forced to use getData since factor cache needs to update when other factors are @@ -70,7 +71,7 @@ end # function Base.getproperty(oas::ObjectAffordanceSubcloud, f::Symbol) # if f == :p_BBo || f == :p_PCloo_blobId # getfield(oas, f) -# elseif f == :ohat_T_p +# elseif f == :bb_T_p # # TODO, slow implementation which is inverting the homograph matrix twice. Make better. # ArrayPartition(oas.p_BBo.position, oas.p_BBo.rotation) # else @@ -80,11 +81,17 @@ end # ObjectAffordanceSubcloud( # p_BBo::_PCL.AbstractBoundingBox, -# ohat_T_p::ArrayPartition, +# bb_T_p::ArrayPartition, # p_PCloo_blobId::UUID -# ) = ObjectAffordanceSubcloud(p_BBo, ArrayPartition(SA[ohat_T_p.x[1]...],SMatrix{size(ohat_T_p.x[2])...}(ohat_T_p.x[2])), p_PCloo_blobId) +# ) = ObjectAffordanceSubcloud(p_BBo, ArrayPartition(SA[bb_T_p.x[1]...],SMatrix{size(bb_T_p.x[2])...}(bb_T_p.x[2])), p_PCloo_blobId) -getManifold(oas::ObjectAffordanceSubcloud) = PowerManifold(getManifold(Pose3Pose3), NestedReplacingPowerRepresentation(), length(oas.ohat_Ts_p)) +getManifold( + oas::ObjectAffordanceSubcloud +) = PowerManifold( + getManifold(Pose3Pose3), + NestedReplacingPowerRepresentation(), + length(oas.bb_Ts_p) +) # NOTE include length of priors which should also be used in measurement updates struct ObjectModelPrior pc::_PCL.PointCloud @@ -126,12 +133,19 @@ function _defaultOASCache( # make static to ensure future updates replace (not all LIEs reuse the same) memory e0 = ArrayPartition( MVector(0.,0.,0.), MMatrix{3,3}(diagm(ones(3))) ) + e0_ = ArrayPartition( SVector(0.,0.,0.), SMatrix{3,3}(diagm(ones(3))) ) APT = typeof(e0) - o_Ts_p = APT[] + bb_Ts_p = Vector{APT}() + o_Ts_bb = Vector{typeof(e0_)}() + # pre-emptively search for any meta object priors + objlbs, o_PC = _findObjPriors(dfg, fvars) + + # start populating the data containers with necessary point clouds and transforms cache = _ObjAffSubcCache(; p_SCs, - o_Ts_p + bb_Ts_p, + o_Ts_bb ) # NOTE, obj variable first, pose variables are [2:end] @@ -139,19 +153,21 @@ function _defaultOASCache( 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]) - ohat_T_p_ = fct.ohat_Ts_p[i] - ohat_T_p = ArrayPartition(MVector(ohat_T_p_.x[1]...), MMatrix{3,3}(ohat_T_p_.x[2])) + bb_T_p_ = fct.bb_Ts_p[i] + bb_T_p = ArrayPartition( + MVector(bb_T_p_.x[1]...), + MMatrix{size(bb_T_p_.x[2])...}(bb_T_p_.x[2]) + ) push!(cache.p_SCs, p_SC) - push!(cache.o_Ts_p, ohat_T_p) + push!(cache.bb_Ts_p, bb_T_p) end - # check and add any priors - objlbs, o_PC = _findObjPriors(dfg, fvars) + # add any priors to back of list if 0 < length(objlbs) # NOTE assume just one model prior allowed (if present) push!(cache.p_SCs, o_PC) - push!(cache.o_Ts_p, e0) + push!(cache.bb_Ts_p, e0) push!(cache.objPrior, objlbs[1]) end @@ -175,15 +191,21 @@ function IncrementalInference.preambleCache( cache = _defaultOASCache(dfg, fvars, fct) # finalize object point clouds for cache # align if there if there is at least one LIE transform and cloud available. - if 0 < length(cache.o_Ts_p) + if 1 < length(cache.bb_Ts_p) # manually set iterLength to skip possible priors, length(fvars) minus the object variable - oo_Ts_p = _PCL.alignPointCloudsLOOIters!( - cache.o_Ts_p, + _PCL.alignPointCloudsLOOIters!( + cache.bb_Ts_p, cache.p_SCs, true, 3; - iterLength=length(cache.o_Ts_p)-length(cache.objPrior) # skip priors + iterLength=length(cache.bb_Ts_p)-length(cache.objPrior) # skip priors ) end + + # Assume fully aligned subclouds, and generate a new common object reference frame + l_PC = _PCL.mergePointCloudsWithTransforms(cache.bb_Ts_p, cache.p_SCs) + l_T_o = _PCL.calcAxes3D(l_PC) + # Chain of frames: from world to pose to localBoundingBox to relativeAlignment to object + # update the cached memory pointers return cache end @@ -199,7 +221,7 @@ function assembleObjectCache( flb::Symbol ) cache = IIF._getCCW(dfg, flb).dummyCache - ohat_SCobj = _PCL.mergePointCloudsWithTransforms(cache.o_Ts_p, cache.p_SCs,) + ohat_SCobj = _PCL.mergePointCloudsWithTransforms(cache.bb_Ts_p, cache.p_SCs,) ohat_SCobj end @@ -210,15 +232,21 @@ function IncrementalInference.getSample( # M = getManifold(cf.factor).manifold e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(diagm(ones(3)))) - iterLength = length(cf.cache.o_Ts_p)-length(cf.cache.objPrior) + iterLength = length(cf.cache.bb_Ts_p)-length(cf.cache.objPrior) Xs = Vector{typeof(e0)}(undef, iterLength) - - # NOTE cache.o_Ts_p and .p_SCs include the model priors at end of lists + + # # TODO generate a common "object frame" -- not the collection of all local bounding box references. + # T_o = if 0 < length(cf.cache.objPrior) + # # use prior reference as object frame reference + # else + # end + + # NOTE cache.bb_Ts_p and .p_SCs include the model priors at end of lists for i in 1:iterLength # TBD consider adding a perturbation before alignment oo_Tloo_p, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( - cf.cache.o_Ts_p, + cf.cache.bb_Ts_p, cf.cache.p_SCs, i; updateTloo=false @@ -269,12 +297,13 @@ function IIF.getMeasurementParametric(foas::DFGFactor{<:CommonConvWrapper{<:Obje e0 = ArrayPartition(SA[0;0;0.], SMatrix{3,3}([1 0 0; 0 1 0; 0 0 1.])) # accept the preambleCache alignment for parametric solves cache = IIF._getCCW(foas).dummyCache - len = length(cache.o_Ts_p) + len = length(cache.bb_Ts_p) μ = zeros(len*D_) # stack all alignments between object and each of the poses to this factor iΣ = zeros(len*D_,len*D_) - iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) - for (i,o_T_p) in enumerate(cache.o_Ts_p) + iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) ## FIXME + for (i,o_T_p) in enumerate(cache.bb_Ts_p) + # FIXME, should reference to a common object frame not the collection of bounding box references. # FIXME, not happy with inv on o_T_p -- OAS factor should read from pose to object??? μ_ = vee(M, e0, log(M, e0, o_T_p )) #inv(M, o_T_p))) idx = D_*(i-1)+1 @@ -305,14 +334,14 @@ function makePointCloudObjectAffordance( fc = getFactor(dfg, flb) cache = IIF._getCCW(fc).dummyCache - o_Ts_p = if align == :fine + bb_Ts_p = if align == :fine @warn("Using preamble fine alignment from $flb") - cache.o_Ts_p + cache.bb_Ts_p else - getFactorType(fc).ohat_Ts_p + getFactorType(fc).bb_Ts_p end - ohat_SCobj = _PCL.mergePointCloudsWithTransforms(o_Ts_p, cache.p_SCs) + ohat_SCobj = _PCL.mergePointCloudsWithTransforms(bb_Ts_p, cache.p_SCs) return ohat_SCobj end @@ -343,12 +372,12 @@ function generateObjectAffordanceFromWorld!( for vlb in vlbs # make sure PPE is set on this solveKey setPPE!(dfg, vlb, solveKey) - p_BBo, p_T_ohat = _PCL.transformFromWorldToLocal(dfg, vlb, w_BBobj; solveKey) # ohat_T_p - ohat_T_p_ = inv(M, p_T_ohat) - ohat_T_p = ArrayPartition(SA[ohat_T_p_.x[1]...], SMatrix{3,3}(ohat_T_p_.x[2])) + p_BBo, p_T_bb = _PCL.transformFromWorldToLocal(dfg, vlb, w_BBobj; solveKey) # bb_T_p + bb_T_p_ = inv(M, p_T_bb) + bb_T_p = ArrayPartition(SA[bb_T_p_.x[1]...], SMatrix{3,3}(bb_T_p_.x[2])) p_PC_blobId = getDataEntry(dfg, vlb, pcBlobLabel).id push!(oas.p_BBos, p_BBo) - push!(oas.ohat_Ts_p, ohat_T_p) + push!(oas.bb_Ts_p, bb_T_p) push!(oas.p_PC_blobIds, p_PC_blobId) end From c904515d35fcca67f87e744ec9b8fb0fd6c8484e Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 13 Jan 2023 15:06:13 -0800 Subject: [PATCH 05/12] better transform naming OAS --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 26 ++++--- src/objects/ObjectAffordanceSubcloud.jl | 72 ++++++++++--------- 2 files changed, 56 insertions(+), 42 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 42fe0e883..198859b69 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -120,21 +120,29 @@ end """ $SIGNATURES -Individually transform and return a merged list of point clouds into one common `::PointCloud` object. +Individually transform and return a merged list of point clouds into a common `::PointCloud` in the local frame from caller. """ function mergePointCloudsWithTransforms( - o_Ts_l::AbstractVector{<:ArrayPartition}, - o_PCs::AbstractVector{<:_PCL.PointCloud}, + l_Ts_bb::AbstractVector{<:ArrayPartition}, + bb_PCs::AbstractVector{<:_PCL.PointCloud}, ) M = SpecialEuclidean(3) - o_PC = _PCL.PointCloud() + l_PC = _PCL.PointCloud() - for (i, l_SC) in enumerate( o_PCs ) - o_SC = _PCL.apply(M, o_Ts_l[i], l_SC) - cat(o_PC, o_SC; reuse=true) + for (l_T_bb, bb_SC) in zip( l_Ts_bb, bb_PCs ) + l_SC = _PCL.apply(M, l_T_bb, bb_SC) + cat(l_PC, l_SC; reuse=true) end - o_PC + # calculate new object frame + l_T_o = _PCL.calcAxes3D(l_PC) + o_T_l = inv(M, l_T_o) + + # calculate transforms from subcloud frames in new object frame o_T_bb + o_Ts_li = map(s->compose(M, o_T_l, s), l_Ts_bb) + + # TODO, change to o_PC + l_PC, o_Ts_li end """ @@ -170,7 +178,7 @@ function alignPointCloudLOO!( ) # prep the leave in elements (all but loo element) lie = setdiff(1:length(l_PCs), [loo_idx;]) - o_PClie = _PCL.mergePointCloudsWithTransforms(ohat_Ts_l[lie], l_PCs[lie]) + o_PClie, o_Ts_li = _PCL.mergePointCloudsWithTransforms(ohat_Ts_l[lie], l_PCs[lie]) # do the alignment of loo onto lie o_Tloo_l_ = if length(ohat_Ts_l) == 1 diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index b624e304c..656879dc8 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -9,9 +9,9 @@ Base.@kwdef struct _ObjAffSubcCache """ list of object subclouds in individual pose reference frames """ p_SCs::Vector{<:_PCL.PointCloud} """ pose to object transforms for each indivudal pose variable """ - bb_Ts_p::Vector{<:ArrayPartition} + lhat_Ts_p::Vector{<:ArrayPartition} """ individual transforms back from each subcloud reference to a common object reference frame """ - o_Ts_bb::Vector{<:ArrayPartition} + o_Ts_li::Vector{<:ArrayPartition} """ any object priors connected to the landmark variable """ objPrior::Vector{Symbol} = Symbol[] # TODO add registers for ObjectAffordancePrior @@ -59,7 +59,7 @@ Base.@kwdef struct ObjectAffordanceSubcloud <: AbstractManifoldMinimize pose to object offset (or pose in object frame) to where the center of the object's bounding box. I.e. the user provided initial guess of relative transform to go from pose to object frame. Note, the type here is restricted to StaticArrays only, to ensure best alignments are stored in the cache. """ - bb_Ts_p::Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))} = Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))}() + lhat_Ts_p::Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))} = Vector{typeof(ArrayPartition(SA[0.;0.;0.],SMatrix{3,3}(diagm([1;1;1.]))))}() """ standard entry blob label where to find the point clouds -- forced to use getData since factor cache needs to update when other factors are @@ -90,7 +90,7 @@ getManifold( ) = PowerManifold( getManifold(Pose3Pose3), NestedReplacingPowerRepresentation(), - length(oas.bb_Ts_p) + length(oas.lhat_Ts_p) ) # NOTE include length of priors which should also be used in measurement updates struct ObjectModelPrior @@ -135,8 +135,8 @@ function _defaultOASCache( e0 = ArrayPartition( MVector(0.,0.,0.), MMatrix{3,3}(diagm(ones(3))) ) e0_ = ArrayPartition( SVector(0.,0.,0.), SMatrix{3,3}(diagm(ones(3))) ) APT = typeof(e0) - bb_Ts_p = Vector{APT}() - o_Ts_bb = Vector{typeof(e0_)}() + lhat_Ts_p = Vector{APT}() + o_Ts_li = Vector{typeof(e0_)}() # pre-emptively search for any meta object priors objlbs, o_PC = _findObjPriors(dfg, fvars) @@ -144,8 +144,8 @@ function _defaultOASCache( # start populating the data containers with necessary point clouds and transforms cache = _ObjAffSubcCache(; p_SCs, - bb_Ts_p, - o_Ts_bb + lhat_Ts_p, + o_Ts_li ) # NOTE, obj variable first, pose variables are [2:end] @@ -153,21 +153,21 @@ function _defaultOASCache( 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]) - bb_T_p_ = fct.bb_Ts_p[i] + bb_T_p_ = fct.lhat_Ts_p[i] bb_T_p = ArrayPartition( MVector(bb_T_p_.x[1]...), MMatrix{size(bb_T_p_.x[2])...}(bb_T_p_.x[2]) ) push!(cache.p_SCs, p_SC) - push!(cache.bb_Ts_p, bb_T_p) + push!(cache.lhat_Ts_p, bb_T_p) end # add any priors to back of list if 0 < length(objlbs) # NOTE assume just one model prior allowed (if present) push!(cache.p_SCs, o_PC) - push!(cache.bb_Ts_p, e0) + push!(cache.lhat_Ts_p, e0) push!(cache.objPrior, objlbs[1]) end @@ -191,21 +191,25 @@ function IncrementalInference.preambleCache( cache = _defaultOASCache(dfg, fvars, fct) # finalize object point clouds for cache # align if there if there is at least one LIE transform and cloud available. - if 1 < length(cache.bb_Ts_p) + if 1 < length(cache.lhat_Ts_p) # manually set iterLength to skip possible priors, length(fvars) minus the object variable _PCL.alignPointCloudsLOOIters!( - cache.bb_Ts_p, + cache.lhat_Ts_p, cache.p_SCs, true, 3; - iterLength=length(cache.bb_Ts_p)-length(cache.objPrior) # skip priors + iterLength=length(cache.lhat_Ts_p)-length(cache.objPrior) # skip priors ) end # Assume fully aligned subclouds, and generate a new common object reference frame - l_PC = _PCL.mergePointCloudsWithTransforms(cache.bb_Ts_p, cache.p_SCs) - l_T_o = _PCL.calcAxes3D(l_PC) + l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(cache.lhat_Ts_p, cache.p_SCs) # Chain of frames: from world to pose to localBoundingBox to relativeAlignment to object - + + # l_T_o = _PCL.calcAxes3D(l_PC) + # M = getManifold(fvars[1]) + # # find all bb_T_o + # compose(M, cache.lhat_Ts_p[i], ArrayPartition(fct.p_BBos[i].position, fct.p_BBos[i].orientation) ) + # update the cached memory pointers return cache end @@ -221,9 +225,9 @@ function assembleObjectCache( flb::Symbol ) cache = IIF._getCCW(dfg, flb).dummyCache - ohat_SCobj = _PCL.mergePointCloudsWithTransforms(cache.bb_Ts_p, cache.p_SCs,) + l_SCobj, o_Ts_li = _PCL.mergePointCloudsWithTransforms(cache.lhat_Ts_p, cache.p_SCs,) - ohat_SCobj + l_SCobj end function IncrementalInference.getSample( @@ -232,7 +236,7 @@ function IncrementalInference.getSample( # M = getManifold(cf.factor).manifold e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(diagm(ones(3)))) - iterLength = length(cf.cache.bb_Ts_p)-length(cf.cache.objPrior) + iterLength = length(cf.cache.lhat_Ts_p)-length(cf.cache.objPrior) Xs = Vector{typeof(e0)}(undef, iterLength) @@ -242,11 +246,11 @@ function IncrementalInference.getSample( # else # end - # NOTE cache.bb_Ts_p and .p_SCs include the model priors at end of lists + # NOTE cache.lhat_Ts_p and .p_SCs include the model priors at end of lists for i in 1:iterLength # TBD consider adding a perturbation before alignment oo_Tloo_p, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( - cf.cache.bb_Ts_p, + cf.cache.lhat_Ts_p, cf.cache.p_SCs, i; updateTloo=false @@ -297,12 +301,12 @@ function IIF.getMeasurementParametric(foas::DFGFactor{<:CommonConvWrapper{<:Obje e0 = ArrayPartition(SA[0;0;0.], SMatrix{3,3}([1 0 0; 0 1 0; 0 0 1.])) # accept the preambleCache alignment for parametric solves cache = IIF._getCCW(foas).dummyCache - len = length(cache.bb_Ts_p) + len = length(cache.lhat_Ts_p) μ = zeros(len*D_) # stack all alignments between object and each of the poses to this factor iΣ = zeros(len*D_,len*D_) iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) ## FIXME - for (i,o_T_p) in enumerate(cache.bb_Ts_p) + for (i,o_T_p) in enumerate(cache.lhat_Ts_p) # FIXME, should reference to a common object frame not the collection of bounding box references. # FIXME, not happy with inv on o_T_p -- OAS factor should read from pose to object??? μ_ = vee(M, e0, log(M, e0, o_T_p )) #inv(M, o_T_p))) @@ -334,16 +338,16 @@ function makePointCloudObjectAffordance( fc = getFactor(dfg, flb) cache = IIF._getCCW(fc).dummyCache - bb_Ts_p = if align == :fine + lhat_Ts_p = if align == :fine @warn("Using preamble fine alignment from $flb") - cache.bb_Ts_p + cache.lhat_Ts_p else - getFactorType(fc).bb_Ts_p + getFactorType(fc).lhat_Ts_p end - ohat_SCobj = _PCL.mergePointCloudsWithTransforms(bb_Ts_p, cache.p_SCs) + l_SCobj, o_Ts_li = _PCL.mergePointCloudsWithTransforms(lhat_Ts_p, cache.p_SCs) - return ohat_SCobj + return l_SCobj end @@ -372,12 +376,14 @@ function generateObjectAffordanceFromWorld!( for vlb in vlbs # make sure PPE is set on this solveKey setPPE!(dfg, vlb, solveKey) - p_BBo, p_T_bb = _PCL.transformFromWorldToLocal(dfg, vlb, w_BBobj; solveKey) # bb_T_p - bb_T_p_ = inv(M, p_T_bb) - bb_T_p = ArrayPartition(SA[bb_T_p_.x[1]...], SMatrix{3,3}(bb_T_p_.x[2])) + # 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.bb_Ts_p, bb_T_p) + push!(oas.lhat_Ts_p, lhat_T_p) push!(oas.p_PC_blobIds, p_PC_blobId) end From e3d576ec9d968147118a98e7f9a093181043531f Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 14 Jan 2023 15:53:59 -0800 Subject: [PATCH 06/12] impr transform names, parametric works, getSample? --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 36 +++++++------- src/objects/ObjectAffordanceSubcloud.jl | 47 +++++++++++++------ 2 files changed, 51 insertions(+), 32 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 198859b69..4737dbeeb 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -158,8 +158,8 @@ Notes - The loo index argument is the element from lists which will be aligned to others. """ function alignPointCloudLOO!( - ohat_Ts_l::AbstractVector{<:ArrayPartition}, - l_PCs::AbstractVector{<:PointCloud}, + ohat_Ts_r::AbstractVector{<:ArrayPartition}, + r_PCs::AbstractVector{<:PointCloud}, loo_idx::Int; updateTloo::Bool=false, verbose=false, @@ -173,18 +173,19 @@ function alignPointCloudLOO!( # prep the leave one out element ohat_PCloo = _PCL.apply( M, - ohat_Ts_l[loo_idx], - l_PCs[loo_idx] + ohat_Ts_r[loo_idx], + r_PCs[loo_idx] ) # prep the leave in elements (all but loo element) - lie = setdiff(1:length(l_PCs), [loo_idx;]) - o_PClie, o_Ts_li = _PCL.mergePointCloudsWithTransforms(ohat_Ts_l[lie], l_PCs[lie]) + lie = setdiff(1:length(r_PCs), [loo_idx;]) + o_PClie, o_Ts_li = _PCL.mergePointCloudsWithTransforms(ohat_Ts_r[lie], r_PCs[lie]) # do the alignment of loo onto lie - o_Tloo_l_ = if length(ohat_Ts_l) == 1 - ohat_Ts_l[1] + o_Tloo_r_ = if length(ohat_Ts_r) == 1 + # take the subcloud reference frame r as the local frame + ohat_Ts_r[1] else - o_Hloo_ohat, Hpts_mov, status = _PCL.alignICP_Simple( + o_Hloo_ohat, _o_PCloo_, status = _PCL.alignICP_Simple( o_PClie, ohat_PCloo; verbose, @@ -199,28 +200,29 @@ function alignPointCloudLOO!( SMatrix{D_,D_}(o_Hloo_ohat[1:D_,1:D_]) ) # that is, take the previous initial estimate and add the new alignment into it - Manifolds.compose( M, o_Tloo_ohat, ohat_Ts_l[loo_idx] ) + Manifolds.compose( M, o_Tloo_ohat, ohat_Ts_r[loo_idx] ) end # make sure we have a static array (this line might be a repeat, but likely worth it) - o_Tloo_l = ArrayPartition( - SVector(o_Tloo_l_.x[1]...), - SMatrix{size(o_Tloo_l_.x[2])...}(o_Tloo_l_.x[2]) + o_Tloo_r = ArrayPartition( + SVector(o_Tloo_r_.x[1]...), + SMatrix{size(o_Tloo_r_.x[2])...}(o_Tloo_r_.x[2]) ) # to confirm the new alignment is good, transform loo cloud from scratch + # note should be the same as _o_PCloo_ above o_PCloo = _PCL.apply( M, - o_Tloo_l, - l_PCs[loo_idx] + o_Tloo_r, + r_PCs[loo_idx] ) if updateTloo @info "updateTloo $loo_idx" - ohat_Ts_l[loo_idx] = o_Tloo_l + ohat_Ts_r[loo_idx] = o_Tloo_r end # TODO confirm expansion around e0, since PosePose factors expand around `q` - return o_Tloo_l, o_PClie, o_PCloo + return o_Tloo_r, o_PClie, o_PCloo end """ diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index 656879dc8..cbdcb54ed 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -133,10 +133,10 @@ function _defaultOASCache( # make static to ensure future updates replace (not all LIEs reuse the same) memory e0 = ArrayPartition( MVector(0.,0.,0.), MMatrix{3,3}(diagm(ones(3))) ) - e0_ = ArrayPartition( SVector(0.,0.,0.), SMatrix{3,3}(diagm(ones(3))) ) + # e0_ = ArrayPartition( SVector(0.,0.,0.), SMatrix{3,3}(diagm(ones(3))) ) APT = typeof(e0) lhat_Ts_p = Vector{APT}() - o_Ts_li = Vector{typeof(e0_)}() + o_Ts_li = Vector{APT}() # pre-emptively search for any meta object priors objlbs, o_PC = _findObjPriors(dfg, fvars) @@ -204,7 +204,13 @@ function IncrementalInference.preambleCache( # Assume fully aligned subclouds, and generate a new common object reference frame l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(cache.lhat_Ts_p, cache.p_SCs) # Chain of frames: from world to pose to localBoundingBox to relativeAlignment to object - + resize!(cache.o_Ts_li, length(o_Ts_li)) + for (i,oTl) in enumerate(o_Ts_li) + cache.o_Ts_li[i] = oTl + # cache.o_Ts_li[i].x[1][:] = oTl.x[1][:] + # cache.o_Ts_li[i].x[2][:] = oTl.x[2][:] + end + # l_T_o = _PCL.calcAxes3D(l_PC) # M = getManifold(fvars[1]) # # find all bb_T_o @@ -236,10 +242,11 @@ function IncrementalInference.getSample( # M = getManifold(cf.factor).manifold e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(diagm(ones(3)))) - iterLength = length(cf.cache.lhat_Ts_p)-length(cf.cache.objPrior) + # LOO iterate over both relatives and priors + iterLength = length(cf.cache.lhat_Ts_p) # - length(cf.cache.objPrior) - Xs = Vector{typeof(e0)}(undef, iterLength) - + # buffer to store LOO updated + lhat_Tloos_p = Vector{typeof(e0)}(undef, iterLength) # # TODO generate a common "object frame" -- not the collection of all local bounding box references. # T_o = if 0 < length(cf.cache.objPrior) # # use prior reference as object frame reference @@ -249,21 +256,30 @@ function IncrementalInference.getSample( # NOTE cache.lhat_Ts_p and .p_SCs include the model priors at end of lists for i in 1:iterLength # TBD consider adding a perturbation before alignment - oo_Tloo_p, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( + # updateTloo=false to allow future multithreading support + lhat_Tloo_p, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( cf.cache.lhat_Ts_p, cf.cache.p_SCs, i; updateTloo=false ) + # TODO confirm expansion around e0, since PosePose factors expand around `q` - X = log(M, e0, oo_Tloo_p) # make sure these are static arrays - Xs[i] = ArrayPartition(SA[X.x[1]...], SMatrix{3,3}(X.x[2])) + lhat_Tloos_p[i] = ArrayPartition(SA[lhat_Tloo_p.x[1]...], SMatrix{3,3}(lhat_Tloo_p.x[2])) end - # FIXME, need better stochastic calculation and representative covariance result in strong unimodal cases -- see this `getMeasurementParametric`` - # return the transform from pose to object as manifold tangent element - return Xs + # Assume fully aligned subclouds, and generate a new common object reference frame + l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(lhat_Tloos_p, cf.cache.p_SCs) + + # TOWARDS stochastic calculation for a strong unimodal object frame -- see `getMeasurementParametric` for this factor + o_Xp = similar(lhat_Tloos_p) + for (i,lTp) in enumerate(lhat_Tloos_p) # cf.cache.o_Ts_li + o_Xp[i] = log(M, e0, Manifolds.compose(M, o_Ts_li[i], lTp)) + end + + # return the transform from pose to object as manifold tangent element + return o_Xp # lhat_Ts_p end @@ -306,10 +322,11 @@ function IIF.getMeasurementParametric(foas::DFGFactor{<:CommonConvWrapper{<:Obje # stack all alignments between object and each of the poses to this factor iΣ = zeros(len*D_,len*D_) iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) ## FIXME - for (i,o_T_p) in enumerate(cache.lhat_Ts_p) + for (i,li_T_p) in enumerate(cache.lhat_Ts_p) # FIXME, should reference to a common object frame not the collection of bounding box references. - # FIXME, not happy with inv on o_T_p -- OAS factor should read from pose to object??? - μ_ = vee(M, e0, log(M, e0, o_T_p )) #inv(M, o_T_p))) + # FIXME, not happy with inv on li_T_p -- OAS factor should read from pose to object??? + # μ_ = vee(M, e0, log(M, e0, Manifolds.compose(M, cache.o_Ts_li[i], li_T_p) )) #inv(M, li_T_p))) + μ_ = vee(M, e0, log(M, e0, li_T_p )) #inv(M, li_T_p))) idx = D_*(i-1)+1 μ[idx:(idx+D_-1)] = μ_ iΣ[idx:(idx+D_-1),idx:(idx+D_-1)] = iΣ_ From a9c29d62cd9173ec0c3640321753f60264429072 Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 14 Jan 2023 19:34:21 -0800 Subject: [PATCH 07/12] more names standardization --- src/objects/ObjectAffordanceSubcloud.jl | 30 ++++++++++++------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index cbdcb54ed..83e23e6b4 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -273,37 +273,37 @@ function IncrementalInference.getSample( l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(lhat_Tloos_p, cf.cache.p_SCs) # TOWARDS stochastic calculation for a strong unimodal object frame -- see `getMeasurementParametric` for this factor - o_Xp = similar(lhat_Tloos_p) + o_Xps = similar(lhat_Tloos_p) for (i,lTp) in enumerate(lhat_Tloos_p) # cf.cache.o_Ts_li - o_Xp[i] = log(M, e0, Manifolds.compose(M, o_Ts_li[i], lTp)) + o_Xps[i] = log(M, e0, Manifolds.compose(M, o_Ts_li[i], lTp)) end # return the transform from pose to object as manifold tangent element - return o_Xp # lhat_Ts_p + return o_Xps # lhat_Ts_p end # FIXME, should be tangent vector not coordinates -- likely part of ManOpt upgrade -function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(X, obj, qs...) +function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(o_Xps, w_T_o, w_Ts_p...) # copied from Pose3Pose3 PM = getManifold(cf.factor) M = PM.manifold # getManifold(Pose3) - e0 = identity_element(M, obj) + e0 = identity_element(M, w_T_o) # NOTE allocalte for vee! see Manifolds #412, fix for AD - q_Cqi = Vector{eltype(obj.x[1])}(undef, 6*length(qs)) - # q_Cqi = zeros(6*length(qs)) - for (i,qi) in enumerate(qs) + p_Cphats = Vector{eltype(w_T_o.x[1])}(undef, 6*length(w_Ts_p)) + # q_Cqi = zeros(6*length(w_Ts_p)) + for (i,w_T_p) in enumerate(w_Ts_p) # work in the group - q̂i = Manifolds.compose(M, obj, exp(M, e0, X[i])) + w_T_phat = Manifolds.compose(M, w_T_o, exp(M, e0, o_Xps[i])) idx = 6*(i-1) - q_Xqi = log(M, qi, q̂i) - # vee!(M, q_Cqi_, qi, q_Xqi) - q_Ci = vee(M, qi, q_Xqi) - q_Cqi[idx+1:idx+6] = q_Ci + # expanding phat around p seems stange? + p_Xphat = log(M, w_T_p, w_T_phat) + p_Cphat = vee(M, w_T_p, p_Xphat) + p_Cphats[idx+1:idx+6] = p_Cphat end # coordinates of all transforms to obj from pose observations - return q_Cqi + return p_Cphats end @@ -321,7 +321,7 @@ function IIF.getMeasurementParametric(foas::DFGFactor{<:CommonConvWrapper{<:Obje μ = zeros(len*D_) # stack all alignments between object and each of the poses to this factor iΣ = zeros(len*D_,len*D_) - iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) ## FIXME + iΣ_ = diagm([10*ones(3); 0.2*ones(3)]) ## FIXME for (i,li_T_p) in enumerate(cache.lhat_Ts_p) # FIXME, should reference to a common object frame not the collection of bounding box references. # FIXME, not happy with inv on li_T_p -- OAS factor should read from pose to object??? From 293912ba6181c1730a0413056d1f9710488ffccf Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 14 Jan 2023 20:03:33 -0800 Subject: [PATCH 08/12] more code and name adj of OAS --- src/objects/ObjectAffordanceSubcloud.jl | 65 +++++++++++++------------ 1 file changed, 33 insertions(+), 32 deletions(-) diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index 83e23e6b4..6b6bab3f1 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -283,30 +283,6 @@ function IncrementalInference.getSample( end -# FIXME, should be tangent vector not coordinates -- likely part of ManOpt upgrade -function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(o_Xps, w_T_o, w_Ts_p...) - # copied from Pose3Pose3 - PM = getManifold(cf.factor) - M = PM.manifold # getManifold(Pose3) - e0 = identity_element(M, w_T_o) - - # NOTE allocalte for vee! see Manifolds #412, fix for AD - p_Cphats = Vector{eltype(w_T_o.x[1])}(undef, 6*length(w_Ts_p)) - # q_Cqi = zeros(6*length(w_Ts_p)) - for (i,w_T_p) in enumerate(w_Ts_p) - # work in the group - w_T_phat = Manifolds.compose(M, w_T_o, exp(M, e0, o_Xps[i])) - idx = 6*(i-1) - # expanding phat around p seems stange? - p_Xphat = log(M, w_T_p, w_T_phat) - p_Cphat = vee(M, w_T_p, p_Xphat) - p_Cphats[idx+1:idx+6] = p_Cphat - end - # coordinates of all transforms to obj from pose observations - return p_Cphats -end - - IIF.getMeasurementParametric(oas::ObjectAffordanceSubcloud) = error("Special case on ObjectAffordanceSubcloud, use lower dispatch `getMeasurementParametric(::DFGFactor{CCW{<:ObjectAffordanceSubcloud}})` instead.") function IIF.getMeasurementParametric(foas::DFGFactor{<:CommonConvWrapper{<:ObjectAffordanceSubcloud}}) @warn "Only artificial inverse covariance available for `getMeasurementParametric(::DFGFactor{CCW{<:ObjectAffordanceSubcloud}})`" maxlog=3 @@ -318,24 +294,49 @@ function IIF.getMeasurementParametric(foas::DFGFactor{<:CommonConvWrapper{<:Obje # accept the preambleCache alignment for parametric solves cache = IIF._getCCW(foas).dummyCache len = length(cache.lhat_Ts_p) - μ = zeros(len*D_) + w_Clps = zeros(len*D_) # stack all alignments between object and each of the poses to this factor - iΣ = zeros(len*D_,len*D_) - iΣ_ = diagm([10*ones(3); 0.2*ones(3)]) ## FIXME + w_iΣ = zeros(len*D_,len*D_) + # inverse covariance matrix, 1/5 on position, 1/0.1 for rotation + w_iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) ## FIXME for (i,li_T_p) in enumerate(cache.lhat_Ts_p) # FIXME, should reference to a common object frame not the collection of bounding box references. # FIXME, not happy with inv on li_T_p -- OAS factor should read from pose to object??? - # μ_ = vee(M, e0, log(M, e0, Manifolds.compose(M, cache.o_Ts_li[i], li_T_p) )) #inv(M, li_T_p))) - μ_ = vee(M, e0, log(M, e0, li_T_p )) #inv(M, li_T_p))) + # w_Clps_ = vee(M, e0, log(M, e0, Manifolds.compose(M, cache.o_Ts_li[i], li_T_p) )) #inv(M, li_T_p))) + w_Clp = vee(M, e0, log(M, e0, li_T_p )) #inv(M, li_T_p))) idx = D_*(i-1)+1 - μ[idx:(idx+D_-1)] = μ_ - iΣ[idx:(idx+D_-1),idx:(idx+D_-1)] = iΣ_ + w_Clps[idx:(idx+D_-1)] = w_Clp + w_iΣ[idx:(idx+D_-1),idx:(idx+D_-1)] = w_iΣ_ end # basically a sort of ProductManifold, but not sure if this interfaces with Manifolds corretly - μ, iΣ + w_Clps, w_iΣ end +# FIXME, should be tangent vector not coordinates -- likely part of ManOpt upgrade +function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(w_Xlps, w_T_o, w_Ts_p...) + # copied from Pose3Pose3 + PM = getManifold(cf.factor) + M = PM.manifold # getManifold(Pose3) + e0 = identity_element(M, w_T_o) + + # NOTE allocalte for vee! see Manifolds #412, fix for AD + p_Cphats = Vector{eltype(w_T_o.x[1])}(undef, 6*length(w_Ts_p)) + # q_Cqi = zeros(6*length(w_Ts_p)) + for (i,w_T_p) in enumerate(w_Ts_p) + # work in the group + l_T_p = exp(M, e0, w_Xlps[i]) + w_T_phat = Manifolds.compose(M, w_T_o, l_T_p) # FIXME, single object frame, not local frame + idx = 6*(i-1) + # expanding phat around p seems stange? + p_Xphat = log(M, w_T_p, w_T_phat) + p_Cphat = vee(M, w_T_p, p_Xphat) + p_Cphats[idx+1:idx+6] = p_Cphat + end + # coordinates of all transforms to obj from pose observations + return p_Cphats +end + ## ================================================================================= ## Object Utils ## ================================================================================= From 5dbaf2e5f33f22de0f81b40d3ce8ee80f3f2f9a4 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 15 Jan 2023 02:10:48 -0800 Subject: [PATCH 09/12] obj axes corrected --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 26 ++++++++++++++++--- src/objects/ObjectAffordanceSubcloud.jl | 18 ++++++------- 2 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 4737dbeeb..1183c52d2 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -124,7 +124,8 @@ Individually transform and return a merged list of point clouds into a common `: """ function mergePointCloudsWithTransforms( l_Ts_bb::AbstractVector{<:ArrayPartition}, - bb_PCs::AbstractVector{<:_PCL.PointCloud}, + bb_PCs::AbstractVector{<:_PCL.PointCloud}; + flipXY::Bool=true ) M = SpecialEuclidean(3) l_PC = _PCL.PointCloud() @@ -135,7 +136,7 @@ function mergePointCloudsWithTransforms( end # calculate new object frame - l_T_o = _PCL.calcAxes3D(l_PC) + l_T_o = _PCL.calcAxes3D(l_PC; flipXY) o_T_l = inv(M, l_T_o) # calculate transforms from subcloud frames in new object frame o_T_bb @@ -329,16 +330,33 @@ iszeros = _PCL.calcAxes3D(_PCL.apply(M, inv(M, l_T_o), pc)) ``` """ function calcAxes3D( - pc::PointCloud + pc::PointCloud; + flipXY::Bool=false ) # xyz_ = (p->[p.x;p.y;p.z]).(pc.points) @cast xyz[d,i] := xyz_[i][d] mdl = fit(PCA, xyz; pratio=1) + # negative because during testing found rotation to have determinant -1 R = projection(mdl) + R .*= det(R) < 0 ? -1 : 1 + @assert isapprox(1, det(R); atol=0.1) "PCA derived rotation matrix should have determinant 1, not $(det(R))." μ = mean(xyz; dims=2) - ArrayPartition(SVector(μ...), SMatrix{3,3}(R)) + R_enh = if !flipXY + # keep the direct PCA R as is + R + else + # try pick a z (vertical) orientation closer to incoming transform + Mr = SpecialOrthogonal(3) + R0 = pc.sensor_orientation_ + Rx = _Rot.RotX(pi)*R + Ry = _Rot.RotY(pi)*R + costs = [distance(Mr, R0, R); distance(Mr, R0, Rx); distance(Mr, R0, Ry)] + (R,Rx,Ry)[argmin(costs)] + end + + ArrayPartition(SVector(μ...), SMatrix{3,3}(R_enh)) end # \ No newline at end of file diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index 6b6bab3f1..26a150a83 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -207,8 +207,6 @@ function IncrementalInference.preambleCache( resize!(cache.o_Ts_li, length(o_Ts_li)) for (i,oTl) in enumerate(o_Ts_li) cache.o_Ts_li[i] = oTl - # cache.o_Ts_li[i].x[1][:] = oTl.x[1][:] - # cache.o_Ts_li[i].x[2][:] = oTl.x[2][:] end # l_T_o = _PCL.calcAxes3D(l_PC) @@ -241,12 +239,12 @@ function IncrementalInference.getSample( ) where {S <: ObjectAffordanceSubcloud} # M = getManifold(cf.factor).manifold - e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(diagm(ones(3)))) + e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(1,0,0,0,1,0,0,0,1.)) # LOO iterate over both relatives and priors iterLength = length(cf.cache.lhat_Ts_p) # - length(cf.cache.objPrior) # buffer to store LOO updated - lhat_Tloos_p = Vector{typeof(e0)}(undef, iterLength) + lhat_TTloo_p = Vector{typeof(e0)}(undef, iterLength) # # TODO generate a common "object frame" -- not the collection of all local bounding box references. # T_o = if 0 < length(cf.cache.objPrior) # # use prior reference as object frame reference @@ -266,20 +264,20 @@ function IncrementalInference.getSample( # TODO confirm expansion around e0, since PosePose factors expand around `q` # make sure these are static arrays - lhat_Tloos_p[i] = ArrayPartition(SA[lhat_Tloo_p.x[1]...], SMatrix{3,3}(lhat_Tloo_p.x[2])) + lhat_TTloo_p[i] = ArrayPartition(SA[lhat_Tloo_p.x[1]...], SMatrix{3,3}(lhat_Tloo_p.x[2])) end # Assume fully aligned subclouds, and generate a new common object reference frame - l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(lhat_Tloos_p, cf.cache.p_SCs) + l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(lhat_TTloo_p, cf.cache.p_SCs) # TOWARDS stochastic calculation for a strong unimodal object frame -- see `getMeasurementParametric` for this factor - o_Xps = similar(lhat_Tloos_p) - for (i,lTp) in enumerate(lhat_Tloos_p) # cf.cache.o_Ts_li - o_Xps[i] = log(M, e0, Manifolds.compose(M, o_Ts_li[i], lTp)) + w_XXop = similar(lhat_TTloo_p) + for (i,lTp) in enumerate(lhat_TTloo_p) # cf.cache.o_Ts_li + w_XXop[i] = log(M, e0, Manifolds.compose(M, o_Ts_li[i], lTp)) end # return the transform from pose to object as manifold tangent element - return o_Xps # lhat_Ts_p + return w_XXop # lhat_Ts_p end From dc5da9374e1f813b6e8480cef3eb20812b266999 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 15 Jan 2023 16:13:59 -0800 Subject: [PATCH 10/12] major rework to ohat o frame, resid TBD --- src/3rdParty/_PCL/services/PointCloudUtils.jl | 2 +- src/objects/ObjectAffordanceSubcloud.jl | 264 ++++++++++-------- 2 files changed, 155 insertions(+), 111 deletions(-) diff --git a/src/3rdParty/_PCL/services/PointCloudUtils.jl b/src/3rdParty/_PCL/services/PointCloudUtils.jl index 1183c52d2..cb9aa6542 100644 --- a/src/3rdParty/_PCL/services/PointCloudUtils.jl +++ b/src/3rdParty/_PCL/services/PointCloudUtils.jl @@ -349,7 +349,7 @@ function calcAxes3D( else # try pick a z (vertical) orientation closer to incoming transform Mr = SpecialOrthogonal(3) - R0 = pc.sensor_orientation_ + R0 = SMatrix{3,3,Float64}(pc.sensor_orientation_) Rx = _Rot.RotX(pi)*R Ry = _Rot.RotY(pi)*R costs = [distance(Mr, R0, R); distance(Mr, R0, Rx); distance(Mr, R0, Ry)] diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index 26a150a83..5d3dc0018 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -6,12 +6,21 @@ # factor for mechanizing object affordances Base.@kwdef struct _ObjAffSubcCache - """ list of object subclouds in individual pose reference frames """ - p_SCs::Vector{<:_PCL.PointCloud} + """ + list of object subclouds in local reference frames + (coarse aligned cloud `lhat_SC = lhat_T_p * p_SC`) + """ + lhat_SCs::Vector{<:_PCL.PointCloud} """ pose to object transforms for each indivudal pose variable """ - lhat_Ts_p::Vector{<:ArrayPartition} - """ individual transforms back from each subcloud reference to a common object reference frame """ - o_Ts_li::Vector{<:ArrayPartition} + ohat_Ts_p::Vector{<:ArrayPartition} + """ same obj subclouds in the object frame """ + ohat_SCs::Vector{<:_PCL.PointCloud} + """ + LOO fine tune alignment here. + transforms from coarse object alignment to single crisp object reference frame + (assuming LOO alignment worked) + """ + o_Ts_ohat::Vector{<:ArrayPartition} """ any object priors connected to the landmark variable """ objPrior::Vector{Symbol} = Symbol[] # TODO add registers for ObjectAffordancePrior @@ -129,23 +138,23 @@ function _defaultOASCache( # use concrete types _PCT(::PC) where {PC <: _PCL.PointCloud} = PC PCT = _PCT(_PCL.PointCloud()) - p_SCs = PCT[] + lhat_SCs = PCT[] + ohat_SCs = PCT[] # make static to ensure future updates replace (not all LIEs reuse the same) memory + M = SpecialEuclidean(3) e0 = ArrayPartition( MVector(0.,0.,0.), MMatrix{3,3}(diagm(ones(3))) ) # e0_ = ArrayPartition( SVector(0.,0.,0.), SMatrix{3,3}(diagm(ones(3))) ) APT = typeof(e0) - lhat_Ts_p = Vector{APT}() - o_Ts_li = Vector{APT}() - - # pre-emptively search for any meta object priors - objlbs, o_PC = _findObjPriors(dfg, fvars) + ohat_Ts_p = Vector{APT}() + o_Ts_ohat = Vector{APT}() # start populating the data containers with necessary point clouds and transforms cache = _ObjAffSubcCache(; - p_SCs, - lhat_Ts_p, - o_Ts_li + lhat_SCs, + ohat_Ts_p, + ohat_SCs, + o_Ts_ohat, ) # NOTE, obj variable first, pose variables are [2:end] @@ -153,21 +162,24 @@ function _defaultOASCache( 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]) - bb_T_p_ = fct.lhat_Ts_p[i] - bb_T_p = ArrayPartition( - MVector(bb_T_p_.x[1]...), - MMatrix{size(bb_T_p_.x[2])...}(bb_T_p_.x[2]) + lhat_T_p_ = fct.lhat_Ts_p[i] + lhat_T_p = ArrayPartition( + MVector(lhat_T_p_.x[1]...), + MMatrix{size(lhat_T_p_.x[2])...}(lhat_T_p_.x[2]) ) - push!(cache.p_SCs, p_SC) - push!(cache.lhat_Ts_p, bb_T_p) + push!(cache.lhat_SCs, _PCL.apply(M, lhat_T_p, p_SC)) + # push!(cache.ohat_Ts_p, e0) end + # any meta object priors + objlbs, op_PC = _findObjPriors(dfg, fvars) + # add any priors to back of list if 0 < length(objlbs) # NOTE assume just one model prior allowed (if present) - push!(cache.p_SCs, o_PC) - push!(cache.lhat_Ts_p, e0) + push!(cache.lhat_SCs, op_PC) + # push!(cache.ohat_Ts_p, e0) push!(cache.objPrior, objlbs[1]) end @@ -188,51 +200,51 @@ function IncrementalInference.preambleCache( fvars::AbstractVector{<:DFGVariable}, fct::ObjectAffordanceSubcloud ) + # construct initialized cache object cache = _defaultOASCache(dfg, fvars, fct) + + M = SpecialEuclidean(3) + e0 = ArrayPartition(MVector(0,0,1.),MMatrix{3,3}(1,0,0,0,1,0,0,0,1.)) + + # define the object frame + lhat_Ts_lhat = typeof(e0)[e0 for _ in 1:length(cache.lhat_SCs)] + lhat_SC, ohat_Ts_lhat = _PCL.mergePointCloudsWithTransforms(lhat_Ts_lhat, cache.lhat_SCs); + + for (i, lhSC) in enumerate(cache.lhat_SCs) + push!( + cache.ohat_SCs, # assumed empty at start + _PCL.apply(M, ohat_Ts_lhat[i], lhSC) + ) + ohat_T_p = Manifolds.compose(M, ohat_Ts_lhat[i], fct.lhat_Ts_p[i]) + push!( + cache.ohat_Ts_p, + ArrayPartition(SVector(ohat_T_p.x[1]...), SMatrix{3,3}(ohat_T_p.x[2])) + ) + end + + for i in 1:length(cache.ohat_SCs) + push!(cache.o_Ts_ohat, e0) + end + # finalize object point clouds for cache # align if there if there is at least one LIE transform and cloud available. - if 1 < length(cache.lhat_Ts_p) + if 1 < length(cache.o_Ts_ohat) # manually set iterLength to skip possible priors, length(fvars) minus the object variable _PCL.alignPointCloudsLOOIters!( - cache.lhat_Ts_p, - cache.p_SCs, - true, 3; - iterLength=length(cache.lhat_Ts_p)-length(cache.objPrior) # skip priors + cache.o_Ts_ohat, + cache.ohat_SCs, + true ) end # Assume fully aligned subclouds, and generate a new common object reference frame - l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(cache.lhat_Ts_p, cache.p_SCs) - # Chain of frames: from world to pose to localBoundingBox to relativeAlignment to object - resize!(cache.o_Ts_li, length(o_Ts_li)) - for (i,oTl) in enumerate(o_Ts_li) - cache.o_Ts_li[i] = oTl - end + o_PC, oo_Ts_ohat = _PCL.mergePointCloudsWithTransforms(cache.o_Ts_ohat, cache.ohat_SCs) + # Chain of frames: from world to pose to local to objhat - # l_T_o = _PCL.calcAxes3D(l_PC) - # M = getManifold(fvars[1]) - # # find all bb_T_o - # compose(M, cache.lhat_Ts_p[i], ArrayPartition(fct.p_BBos[i].position, fct.p_BBos[i].orientation) ) - # update the cached memory pointers return cache end -""" - $SIGNATURES - -Debug tool to check values in the cache object make sense. -Use before and after [`alignPointCloudsLOOIters!`](@ref). -""" -function assembleObjectCache( - dfg::AbstractDFG, - flb::Symbol -) - cache = IIF._getCCW(dfg, flb).dummyCache - l_SCobj, o_Ts_li = _PCL.mergePointCloudsWithTransforms(cache.lhat_Ts_p, cache.p_SCs,) - - l_SCobj -end function IncrementalInference.getSample( cf::CalcFactor{S}, @@ -241,43 +253,39 @@ function IncrementalInference.getSample( M = getManifold(cf.factor).manifold e0 = ArrayPartition(SVector(1,1,1.),SMatrix{3,3}(1,0,0,0,1,0,0,0,1.)) # LOO iterate over both relatives and priors - iterLength = length(cf.cache.lhat_Ts_p) # - length(cf.cache.objPrior) + iterLength = length(cf.cache.lhat_SCs) # buffer to store LOO updated - lhat_TTloo_p = Vector{typeof(e0)}(undef, iterLength) - # # TODO generate a common "object frame" -- not the collection of all local bounding box references. - # T_o = if 0 < length(cf.cache.objPrior) - # # use prior reference as object frame reference - # else - # end - - # NOTE cache.lhat_Ts_p and .p_SCs include the model priors at end of lists + o_Tsloo_ohat = Vector{typeof(e0)}(undef, iterLength) + + # NOTE cache.ohat_Ts_p and .lhat_SCs include the model priors at end of lists for i in 1:iterLength # TBD consider adding a perturbation before alignment # updateTloo=false to allow future multithreading support - lhat_Tloo_p, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( - cf.cache.lhat_Ts_p, - cf.cache.p_SCs, + o_Tsloo_ohat_, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( + cf.cache.o_Ts_ohat, + cf.cache.ohat_SCs, i; updateTloo=false ) # TODO confirm expansion around e0, since PosePose factors expand around `q` # make sure these are static arrays - lhat_TTloo_p[i] = ArrayPartition(SA[lhat_Tloo_p.x[1]...], SMatrix{3,3}(lhat_Tloo_p.x[2])) + o_Tsloo_ohat[i] = ArrayPartition(SA[o_Tsloo_ohat_.x[1]...], SMatrix{3,3}(o_Tsloo_ohat_.x[2])) end # Assume fully aligned subclouds, and generate a new common object reference frame - l_PC, o_Ts_li = _PCL.mergePointCloudsWithTransforms(lhat_TTloo_p, cf.cache.p_SCs) + o_PC, oo_Ts_ohat = _PCL.mergePointCloudsWithTransforms(o_Tsloo_ohat, cf.cache.ohat_SCs) # TOWARDS stochastic calculation for a strong unimodal object frame -- see `getMeasurementParametric` for this factor - w_XXop = similar(lhat_TTloo_p) - for (i,lTp) in enumerate(lhat_TTloo_p) # cf.cache.o_Ts_li - w_XXop[i] = log(M, e0, Manifolds.compose(M, o_Ts_li[i], lTp)) + w_XXop = similar(o_Tsloo_ohat) + for (i,lTp) in enumerate(o_Tsloo_ohat) + # FIXME THIS IS NOT RIGHT + w_XXop[i] = log(M, e0, Manifolds.compose(M, oo_Ts_ohat[i], lTp)) end # return the transform from pose to object as manifold tangent element - return w_XXop # lhat_Ts_p + return w_XXop end @@ -288,26 +296,25 @@ function IIF.getMeasurementParametric(foas::DFGFactor{<:CommonConvWrapper{<:Obje PM = getManifold(getFactorType(foas)) M = PM.manifold D_ = manifold_dimension(M) - e0 = ArrayPartition(SA[0;0;0.], SMatrix{3,3}([1 0 0; 0 1 0; 0 0 1.])) + e0 = ArrayPartition(SA[0;0;0.], SMatrix{3,3}(1, 0, 0, 0, 1, 0, 0, 0, 1.)) # accept the preambleCache alignment for parametric solves cache = IIF._getCCW(foas).dummyCache - len = length(cache.lhat_Ts_p) - w_Clps = zeros(len*D_) + len = length(cache.lhat_SCs) + w_Cops = zeros(len*D_) # stack all alignments between object and each of the poses to this factor w_iΣ = zeros(len*D_,len*D_) # inverse covariance matrix, 1/5 on position, 1/0.1 for rotation - w_iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) ## FIXME - for (i,li_T_p) in enumerate(cache.lhat_Ts_p) - # FIXME, should reference to a common object frame not the collection of bounding box references. - # FIXME, not happy with inv on li_T_p -- OAS factor should read from pose to object??? - # w_Clps_ = vee(M, e0, log(M, e0, Manifolds.compose(M, cache.o_Ts_li[i], li_T_p) )) #inv(M, li_T_p))) - w_Clp = vee(M, e0, log(M, e0, li_T_p )) #inv(M, li_T_p))) + w_iΣ_ = diagm([0.2*ones(3); 10*ones(3)]) + for i in 1:len + # OAS factor should read from pose to object + o_T_p = Manifolds.compose(M, cache.o_Ts_ohat[i], cache.ohat_Ts_p[i]) + w_Cop = vee(M, e0, log(M, e0, o_T_p )) #inv(M, li_T_p))) idx = D_*(i-1)+1 - w_Clps[idx:(idx+D_-1)] = w_Clp + w_Cops[idx:(idx+D_-1)] = w_Cop w_iΣ[idx:(idx+D_-1),idx:(idx+D_-1)] = w_iΣ_ end # basically a sort of ProductManifold, but not sure if this interfaces with Manifolds corretly - w_Clps, w_iΣ + w_Cops, w_iΣ end @@ -335,38 +342,12 @@ function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(w_Xlps, w_T_o, w_Ts_p...) return p_Cphats end + ## ================================================================================= -## Object Utils +## Graph construction helpers ## ================================================================================= -""" - $SIGNATURES - -Put the object subclouds together in the object frame based on info stored in the factor graph -around the object variable `ovl`. -""" -function makePointCloudObjectAffordance( - dfg::AbstractDFG, - flb::Symbol; - align=:fine # or :coarse -) - fc = getFactor(dfg, flb) - cache = IIF._getCCW(fc).dummyCache - - lhat_Ts_p = if align == :fine - @warn("Using preamble fine alignment from $flb") - cache.lhat_Ts_p - else - getFactorType(fc).lhat_Ts_p - end - - l_SCobj, o_Ts_li = _PCL.mergePointCloudsWithTransforms(lhat_Ts_p, cache.p_SCs) - - return l_SCobj -end - - function generateObjectAffordanceFromWorld!( dfg::AbstractDFG, olb::Symbol, @@ -449,4 +430,67 @@ function reducePoseGraphOnObjectAffordances( reducePoseGraphOnObjectAffordances!(dest, src) end + +## ================================================================================= +## Object Utils +## ================================================================================= + +""" + $SIGNATURES + +Debug tool to check values in the cache object make sense. +Use before and after [`alignPointCloudsLOOIters!`](@ref). +""" +function assembleObjectCache( + cache::_ObjAffSubcCache +) + o_SCobj, o_Ts_li = _PCL.mergePointCloudsWithTransforms( + cache.o_Ts_ohat, + cache.ohat_SCs + ) + # l_SCobj, o_Ts_li = _PCL.mergePointCloudsWithTransforms(cache.ohat_Ts_p, cache.lhat_SCs,) + + o_SCobj +end +assembleObjectCache(dfg::AbstractDFG, flb::Symbol) = assembleObjectCache(IIF._getCCW(dfg, flb).dummyCache) + +""" + $SIGNATURES + +Put the object subclouds together in the object frame based on info stored in the factor graph +around the object variable `ovl`. +""" +function makePointCloudObjectAffordance( + dfg::AbstractDFG, + flb::Symbol; + align=:fine # or :coarse +) + fc = getFactor(dfg, flb) + cache = IIF._getCCW(fc).dummyCache + + _SCobj = if align === :fine + @info("Using preamble fine alignment from $flb") + assembleObjectCache(cache) + elseif align === :coarse + M = SpecialEuclidean(3) + e0 = ArrayPartition(SVector(0,0,0.),SMatrix{3,3}(1,0,0,0,1,0,0,0,1.)) + lhat_Ts_lhat = Vector{typeof(e0)}() + for (i,lTp) in enumerate(getFactorType(fc).lhat_Ts_p) + # pTp = Manifolds.compose(M, inv(M, lTp), cache.ohat_Ts_p[i]) + # FIXME, why pTp not identity? Did cache.ohat_Ts_p get updated in an alignLOO somewhere? + # @assert isapprox(M, e0, pTp) "Cached lTp transform not what user provided, $flb" + push!(lhat_Ts_lhat, e0) + end + # using lhat_T_lhat since lhat_SCs point clouds already rotated into coarse local frame lhat + _SCobj, _ = _PCL.mergePointCloudsWithTransforms( + lhat_Ts_lhat, + cache.lhat_SCs + ) + _SCobj + end + + return _SCobj +end + + ## \ No newline at end of file From cf888739d12f754f22f14d298d9c901950fcb332 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 15 Jan 2023 18:33:20 -0800 Subject: [PATCH 11/12] update resid for ohat and o references --- src/objects/ObjectAffordanceSubcloud.jl | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index 5d3dc0018..dbf2cb025 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -238,7 +238,7 @@ function IncrementalInference.preambleCache( end # Assume fully aligned subclouds, and generate a new common object reference frame - o_PC, oo_Ts_ohat = _PCL.mergePointCloudsWithTransforms(cache.o_Ts_ohat, cache.ohat_SCs) + # o_PC, oo_Ts_ohat = _PCL.mergePointCloudsWithTransforms(cache.o_Ts_ohat, cache.ohat_SCs) # Chain of frames: from world to pose to local to objhat # update the cached memory pointers @@ -262,6 +262,7 @@ function IncrementalInference.getSample( for i in 1:iterLength # TBD consider adding a perturbation before alignment # updateTloo=false to allow future multithreading support + # updateTloo=false prevents solution from driftinging during solve, each sampling is equivalent from when object was created o_Tsloo_ohat_, o_PClie, o_PCloo = _PCL.alignPointCloudLOO!( cf.cache.o_Ts_ohat, cf.cache.ohat_SCs, @@ -274,18 +275,19 @@ function IncrementalInference.getSample( o_Tsloo_ohat[i] = ArrayPartition(SA[o_Tsloo_ohat_.x[1]...], SMatrix{3,3}(o_Tsloo_ohat_.x[2])) end - # Assume fully aligned subclouds, and generate a new common object reference frame - o_PC, oo_Ts_ohat = _PCL.mergePointCloudsWithTransforms(o_Tsloo_ohat, cf.cache.ohat_SCs) + # FIXME, is it better to use new obj reference frame? + # # Assume fully aligned subclouds, and generate a new common object reference frame + # o_PC, oo_Ts_ohat = _PCL.mergePointCloudsWithTransforms(o_Tsloo_ohat, cf.cache.ohat_SCs) # TOWARDS stochastic calculation for a strong unimodal object frame -- see `getMeasurementParametric` for this factor - w_XXop = similar(o_Tsloo_ohat) - for (i,lTp) in enumerate(o_Tsloo_ohat) + w_Xops = similar(o_Tsloo_ohat) + for (i,ohTp) in enumerate(cf.cache.ohat_Ts_p) # FIXME THIS IS NOT RIGHT - w_XXop[i] = log(M, e0, Manifolds.compose(M, oo_Ts_ohat[i], lTp)) + w_Xops[i] = log(M, e0, Manifolds.compose(M, o_Tsloo_ohat[i], ohTp)) end # return the transform from pose to object as manifold tangent element - return w_XXop + return w_Xops end @@ -319,7 +321,7 @@ end # FIXME, should be tangent vector not coordinates -- likely part of ManOpt upgrade -function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(w_Xlps, w_T_o, w_Ts_p...) +function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(w_Xops, w_T_o, w_Ts_p...) # copied from Pose3Pose3 PM = getManifold(cf.factor) M = PM.manifold # getManifold(Pose3) @@ -330,8 +332,8 @@ function (cf::CalcFactor{<:ObjectAffordanceSubcloud})(w_Xlps, w_T_o, w_Ts_p...) # q_Cqi = zeros(6*length(w_Ts_p)) for (i,w_T_p) in enumerate(w_Ts_p) # work in the group - l_T_p = exp(M, e0, w_Xlps[i]) - w_T_phat = Manifolds.compose(M, w_T_o, l_T_p) # FIXME, single object frame, not local frame + o_T_p = exp(M, e0, w_Xops[i]) + w_T_phat = Manifolds.compose(M, w_T_o, o_T_p) # FIXME, single object frame, not local frame idx = 6*(i-1) # expanding phat around p seems stange? p_Xphat = log(M, w_T_p, w_T_phat) From c71642f414c893d44e36ba878a890474fb859737 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 15 Jan 2023 19:03:31 -0800 Subject: [PATCH 12/12] impr comment --- src/objects/ObjectAffordanceSubcloud.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/objects/ObjectAffordanceSubcloud.jl b/src/objects/ObjectAffordanceSubcloud.jl index dbf2cb025..4892332e3 100644 --- a/src/objects/ObjectAffordanceSubcloud.jl +++ b/src/objects/ObjectAffordanceSubcloud.jl @@ -282,7 +282,7 @@ function IncrementalInference.getSample( # TOWARDS stochastic calculation for a strong unimodal object frame -- see `getMeasurementParametric` for this factor w_Xops = similar(o_Tsloo_ohat) for (i,ohTp) in enumerate(cf.cache.ohat_Ts_p) - # FIXME THIS IS NOT RIGHT + # TODO confirm pose to init object reference frame transform logic w_Xops[i] = log(M, e0, Manifolds.compose(M, o_Tsloo_ohat[i], ohTp)) end