From d4e814134deffbac55741263236b23f6b1d1ee80 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 15 Mar 2022 20:20:50 -0700 Subject: [PATCH 1/3] CF.cache not being set? --- src/factors/Pose2D.jl | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index bb6d0d17..116bd7fa 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -20,16 +20,26 @@ DFG.getManifold(::InstanceType{Pose2Pose2}) = getManifold(Pose2) # Manifolds.Spe Pose2Pose2(::UniformScaling) = Pose2Pose2() +function preambleCache(pp::Pose2Pose2) + M = getManifold(pp) + (;manifold=M, ϵ0=identity_element(M), Xc=zeros(3)) +end # Assumes X is a tangent vector function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) @assert X isa ProductRepr "Pose2Pose2 expects measurement sample X to be a Manifolds tangent vector, not coordinate or point representation. Got X=$X" - M = getManifold(Pose2) - q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups + + @info "HERE" cf.cache + M = cf.cache.manifold # getManifold(Pose2) + ϵ0 = cf.cache.ϵ0 + + q̂ = Manifolds.compose(M, p, exp(M, ϵ0, X)) #for groups + # q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups #TODO allocalte for vee! see Manifolds #412, fix for AD - Xc = zeros(3) - vee!(M, Xc, q, log(M, q, q̂)) - return Xc + fill!(cf.cache.Xc, 0.0) + # Xc = zeros(3) + vee!(M, cf.cache.Xc, q, log(M, q, q̂)) + return cf.cache.Xc end From f4d76106ae43d9c64036e90f58b5e9d747535e0b Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 22 Mar 2022 20:01:52 -0700 Subject: [PATCH 2/3] fix Pose2 cache problem --- src/factors/Pose2D.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index 116bd7fa..a3c4f4c1 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -20,7 +20,7 @@ DFG.getManifold(::InstanceType{Pose2Pose2}) = getManifold(Pose2) # Manifolds.Spe Pose2Pose2(::UniformScaling) = Pose2Pose2() -function preambleCache(pp::Pose2Pose2) +function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, pp::Pose2Pose2) M = getManifold(pp) (;manifold=M, ϵ0=identity_element(M), Xc=zeros(3)) end @@ -29,7 +29,7 @@ end function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) @assert X isa ProductRepr "Pose2Pose2 expects measurement sample X to be a Manifolds tangent vector, not coordinate or point representation. Got X=$X" - @info "HERE" cf.cache + # @info "HERE" cf.cache M = cf.cache.manifold # getManifold(Pose2) ϵ0 = cf.cache.ϵ0 From 840c3fecb0b06487e976ea32ad4ac4967fc278ba Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 24 Mar 2022 11:23:28 -0700 Subject: [PATCH 3/3] add q_hat/ --- src/factors/Pose2D.jl | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index a3c4f4c1..827854cd 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -22,26 +22,24 @@ Pose2Pose2(::UniformScaling) = Pose2Pose2() function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, pp::Pose2Pose2) M = getManifold(pp) - (;manifold=M, ϵ0=identity_element(M), Xc=zeros(3)) + (;manifold=M, ϵ0=identity_element(M), Xc=zeros(3), q̂=identity_element(M)) end # Assumes X is a tangent vector function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) - @assert X isa ProductRepr "Pose2Pose2 expects measurement sample X to be a Manifolds tangent vector, not coordinate or point representation. Got X=$X" - - # @info "HERE" cf.cache + @assert X isa ProductRepr || X isa Manifolds.ArrayPartition "Pose2Pose2 expects measurement sample X to be a Manifolds tangent vector, not coordinate or point representation. Got X=$X" + M = cf.cache.manifold # getManifold(Pose2) ϵ0 = cf.cache.ϵ0 - - q̂ = Manifolds.compose(M, p, exp(M, ϵ0, X)) #for groups - # q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups - #TODO allocalte for vee! see Manifolds #412, fix for AD + q̂ = cf.cache.q̂ + + #for groups + Manifolds.compose!(M, q̂, p, exp(M, ϵ0, X)) fill!(cf.cache.Xc, 0.0) - # Xc = zeros(3) vee!(M, cf.cache.Xc, q, log(M, q, q̂)) return cf.cache.Xc end - + # NOTE, serialization support -- will be reduced to macro in future