Merge pull request #1329 from JuliaRobotics/21Q3/fix/testpartials

wip toward ipc via perturbation on relative

Author: dehann

src/ApproxConv.jl

@@ -376,7 +376,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
                                 skipSolve::Bool=false,
                                 _slack=nothing  ) where {T <: AbstractFactor}
   #
-  # @info "EVALSPEC" string(measurement) inflateCycles
+  
   # Prep computation variables
   # NOTE #1025, should FMD be built here...
   sfidx, maxlen, mani = prepareCommonConvWrapper!(ccwl, Xi, solvefor, N, needFreshMeasurements=needFreshMeasurements, solveKey=solveKey)
@@ -408,7 +408,13 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
                             _slack=_slack )
   #
   # do info per coord
-  ipc = ones(getDimension(Xi[sfidx]))
+  ipc = if ccwl._gradients === nothing 
+    ones(getDimension(Xi[sfidx]))
+  else
+    # calcPerturbationFromVariable(ccwl._gradients, 2, ipc_)
+    ones(getDimension(Xi[sfidx]))
+  end
+
   # return the found points, and info per coord
   return ccwl.params[ccwl.varidx], ipc
 end

src/FactorGraph.jl

@@ -631,17 +631,17 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
   pttypes = getVariableType.(Xi) .|> getPointType
   PointType = 0 < length(pttypes) ? pttypes[1] : Vector{Float64}
   # FIXME stop using Any, see #1321
-  ARR = Vector{Vector{Any}}()
-  maxlen, sfidx, mani = prepareparamsarray!(ARR, Xi, nothing, 0) # Nothing for init.
+  varParams = Vector{Vector{Any}}()
+  maxlen, sfidx, mani = prepareparamsarray!(varParams, Xi, nothing, 0) # Nothing for init.
 
   # standard factor metadata
   sflbl = 0==length(Xi) ? :null : getLabel(Xi[end])
-  fmd = FactorMetadata(Xi, getLabel.(Xi), ARR, sflbl, nothing)
+  fmd = FactorMetadata(Xi, getLabel.(Xi), varParams, sflbl, nothing)
 
   # create a temporary CalcFactor object for extracting the first sample
   # TODO, deprecate this:  guess measurement points type
   # MeasType = Vector{Float64} # FIXME use `usrfnc` to get this information instead
-  _cf = CalcFactor( usrfnc, fmd, 0, 1, nothing, ARR) # (Vector{MeasType}(),)
+  _cf = CalcFactor( usrfnc, fmd, 0, 1, nothing, varParams) # (Vector{MeasType}(),)
 
   # get a measurement sample
   meas_single = sampleFactor(_cf, 1)
@@ -658,12 +658,13 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
     Int[]
   end
 
-  varTypes = typeof.(getVariableType.(Xi))
+  # as per struct CommonConvWrapper
+  varTypes::Vector{DataType} = typeof.(getVariableType.(Xi))
   gradients = nothing
   # prepare new cached gradient lambdas (attempt)
   # try
   #   measurement = tuple(((x->x[1]).(meas_single))...)
-  #   pts = tuple(((x->x[1]).(ARR))...)
+  #   pts = tuple(((x->x[1]).(varParams))...)
   #   gradients = FactorGradientsCached!(usrfnc, varTypes, measurement, pts);
   # catch e
   #   @warn "Unable to create measurements and gradients for $usrfnc during prep of CCW, falling back on no-partial information assumption."
@@ -673,7 +674,7 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
           usrfnc,
           PointType[],
           zdim,
-          ARR,
+          varParams,
           fmd,
           specialzDim = hasfield(T, :zDim),
           partial = ispartl,

src/FactorGraphTypes.jl

@@ -300,7 +300,7 @@ function CommonConvWrapper( fnc::T,
                                               perturb=perturb, res=res )).(1:Threads.nthreads()),
                             inflation,
                             partialDims,  # SVector(Int32.()...)
-                            vartypes,
+                            DataType[vartypes...],
                             gradients)
 end
 

src/entities/FactorGradients.jl

@@ -71,6 +71,8 @@ mutable struct FactorGradientsCached!{F <: AbstractRelative,S,M<:Tuple,P,G,L}
   # nested-tuple of gradient lambda functions
   _λ_fncs::L
   _coord_sizes::Vector{Int}
+  # gradient delta
+  _h::Float64
 end
 
 

src/services/FactorGradients.jl

@@ -1,6 +1,7 @@
 # utilities for calculating the gradient over factors
 
 export getCoordSizes
+export checkGradientsToleranceMask, calcPerturbationFromVariable
 
 
 # T_pt_args[:] = [(T1::Type{<:InferenceVariable}, point1); ...]
@@ -96,7 +97,8 @@ function FactorGradientsCached!(fct::Union{<:AbstractRelativeMinimize, <:Abstrac
                           pts,
                           tfg,
                           λ_fncs,
-                          λ_sizes )
+                          λ_sizes,
+                          h )
 end
 
 
@@ -148,4 +150,105 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
 
   # return newly calculated gradients
   return fgc.cached_gradients
-end
+end
+
+"""
+    $SIGNATURES
+
+Return a mask of same size as gradients matrix `J`, indicating which elements are above the expected sensitivity threshold `tol`.
+
+Notes
+- Threshold accuracy depends on two parts,
+  - Numerical gradient perturbation size `fgc._h`,
+  - Accuracy tolerance to which the factor residual is computed (not controlled here)
+"""
+function checkGradientsToleranceMask( fgc::FactorGradientsCached!, 
+                                      J::AbstractArray=fgc.cached_gradients;
+                                      tol::Real=0.02*fgc._h )
+  #
+  # ignore anything 10 times smaller than numerical gradient delta used
+  # NOTE this ignores the factor residual solve accuracy
+  return tol*fgc._h .< abs.(J)
+end
+
+"""
+    $SIGNATURES
+
+Return a tuple of infoPerCoord vectors that result from input `fromVar::Int`'s `infoPerCoord`.
+For example, a binary `LinearRelative` factor has a one to one influence from the input to the one other variable.
+
+Notes
+
+- Assumes the gradients in `fgc` are up to date -- if not, first run `fgc(measurement..., pts...)`.
+- `tol` does not recalculate the gradients to a new tolerance, instead uses the cached value in `fgc` to predict accuracy.
+
+Example
+
+```julia
+# setup
+fct = LinearRelative(MvNormal([10;0],[1 0; 0 1]))
+measurement = ([10.0;0.0],)
+varTypes = (ContinuousEuclid{2}, ContinuousEuclid{2})
+pts = ([0;0.0], [9.5;0])
+
+# create the gradients functor object
+fgc = FactorGradientsCached!(fct, varTypes, measurement, pts);
+# must first update the cached gradients
+fgc(measurement..., pts...)
+
+# check the perturbation influence through gradients on factor
+ret = calcPerturbationFromVariable(fgc, 1, [1;1])
+
+@assert isapprox(ret[2], [1;1])
+```
+
+DevNotes
+- FIXME Support n-ary source factors by extending `fromVar` to more than just one. 
+
+Related
+
+[`FactorGradientsCached!`](@ref), [`checkGradientsToleranceMask`](@ref)
+"""
+function calcPerturbationFromVariable(fgc::FactorGradientsCached!, 
+                                      fromVar::Int, 
+                                      infoPerCoord::AbstractVector;
+                                      tol::Real=0.02*fgc._h )
+  #
+  blkszs = getCoordSizes(fgc)
+  @assert blkszs[fromVar] == length(infoPerCoord) "Expecting incoming length(infoPerCoord) to equal the block size for variable $fromVar, as per factor used to construct the FactorGradientsCached!: $(getFactorType(fgc.dfgfct))"
+  # assume projection through pp-factor from first to second variable 
+  # ipc values from first variable belief, and zero for second
+  ipcAll = zeros(sum(blkszs))
+
+  # nextVar = minimum([fromVar+1; length(blkszs)])
+  curr_b = sum(blkszs[1:(fromVar-1)]) + 1
+  curr_e = sum(blkszs[1:fromVar])
+  ipcAll[curr_b:curr_e] .= infoPerCoord
+  
+  # clamp gradients below numerical solver resolution
+  mask = checkGradientsToleranceMask(fgc, tol=tol)
+  J = fgc.cached_gradients
+  _J = zeros(size(J)...)
+  _J[mask] .= J[mask]
+
+  # calculate the gradient influence on other variables
+  ipc_pert = _J * ipcAll
+
+  # round up over numerical solution tolerance
+  dig = floor(Int, log10(1/tol))
+  ipc_pert .= round.(ipc_pert, digits=dig)
+
+  # slice the result
+  ipcBlk = []
+  for (i, sz) in enumerate(blkszs)
+    curr_b = sum(blkszs[1:(i-1)]) + 1
+    curr_e = sum(blkszs[1:i])
+    blk_ = view(ipc_pert, curr_b:curr_e)
+    push!(ipcBlk, blk_)
+  end
+
+  return tuple(ipcBlk...)
+end
+
+
+#

test/runtests.jl

@@ -6,14 +6,14 @@ using Test
 # temporarily moved to start (for debugging)
 
 # @error "MUST RESTORE testpartialconstraint.jl"
+include("testFactorGradients.jl")
 include("testpartialconstraint.jl")
 include("testPartialNH.jl")
 
 include("testSphereMani.jl")
 include("testSpecialOrthogonalMani.jl")
 include("testSpecialEuclidean2Mani.jl")
 
-include("testFactorGradients.jl")
 
 include("testCliqSolveDbgUtils.jl")
 include("TestModuleFunctions.jl")

test/testFactorGradients.jl

@@ -6,6 +6,9 @@ using TensorCast
 using Manifolds
 using Test
 
+# overloading with new dispatch
+import IncrementalInference: getSample, getManifold
+
 ##
 
 @testset "test manual call to gradient lambda utilities" begin
@@ -48,8 +51,82 @@ J = gradFct(measurement..., pts...)
 
 @test norm( J - [0 0 1 0; 0 0 0 1; 1 0 0 0; 0 1 0 0] ) < 1e-4
 
+## check on transmitted info per coords
+
+ret = calcPerturbationFromVariable(gradFct, 1, [1;1])
+
+# the fromVar itself should be zero
+@test length(ret[1]) == 2
+@test isapprox( ret[1], [0;0], atol=1e-6 )
+
+# the other variable
+@test length(ret[2]) == 2
+@test isapprox( ret[2], [1;1], atol=1e-6 )
 
 ##
 end
 
+##
+
+struct TestPartialRelative2D{B <: SamplableBelief} <: IIF.AbstractRelativeMinimize
+  Z::B
+  partial::Tuple{Int}
+end
+# standard helper with partial set
+TestPartialRelative2D(z::SamplableBelief) = TestPartialRelative2D(z, (2,))
+
+# imported earlier for overload
+getManifold(fnc::TestPartialRelative2D) = TranslationGroup(2)
+getSample(cf::CalcFactor{<:TestPartialRelative2D},N=1) = ([rand(cf.factor.Z) for _ in 1:N], )
+
+# currently requires residual to be returned as a tangent vector element
+(cf::CalcFactor{<:TestPartialRelative2D})(z, x1, x2) = x2[2:2] - (x1[2:2] + z[1:1])
+
+##
+
+@testset "test a partial, binary relative factor perturbation (a new user factor)" begin
+##
+
+
+tpr = TestPartialRelative2D(Normal(10,1))
+
+measurement = ([10.0;], )
+pts = ([0;0.0], [0;10.0])
+varTypes = (ContinuousEuclid{2}, ContinuousEuclid{2})
+
+## construct the lambdas
+gradients = FactorGradientsCached!(tpr, varTypes, measurement, pts);
+
+## calculate new gradients
+J = gradients(measurement..., pts...)
+
+
+## check on transmitted info per coords
+
+ret = calcPerturbationFromVariable(gradients, 1, [1;1])
+
+# the fromVar itself should be zero
+@test length(ret[1]) == 2
+@test isapprox( ret[1], [0;0], atol=1e-6 )
+
+# the other variable only affects the second coordinate dimension
+@test length(ret[2]) == 2
+@test isapprox( ret[2], [0;1], atol=1e-6 )
+
+## check the reverse perturbation
+
+ret = calcPerturbationFromVariable(gradients, 2, [1;1])
+
+# only the first coordinate dimension is affected
+@test length(ret[1]) == 2
+@test isapprox( ret[1], [0;1], atol=1e-6 )
+
+## the fromVar itself should be zero
+@test length(ret[2]) == 2
+@test isapprox( ret[2], [0;0], atol=1e-6 )
+
+##
+end
+
+
 #

test/testGradientUtils.jl

@@ -8,18 +8,19 @@ using Test
 ## test utility to build a temporary graph
 
 fct = EuclidDistance(Normal(10,1))
-T_pt_vec = [(ContinuousScalar,ContinuousScalar); ([0;],[9.5;])]
+varTypes = (ContinuousScalar,ContinuousScalar); 
+varPts = ([0;],[9.5;])
 
 ##
 
-dfg, _dfgfct = IIF._buildGraphByFactorAndTypes!(fct, T_pt_vec...)
+dfg, _dfgfct = IIF._buildGraphByFactorAndTypes!(fct, varTypes, varPts)
 
 @test length(intersect(ls(dfg), [:x1; :x2])) == 2
 @test lsf(dfg) == [:x1x2f1;]
 
 ## test  the evaluation of factor without
 
-B = IIF._evalFactorTemporary!(EuclidDistance(Normal(10,1)), 2, ([10;],), T_pt_vec... );
+B = IIF._evalFactorTemporary!(EuclidDistance(Normal(10,1)), 2, ([10;],), varTypes, varPts );
 
 @test_broken B isa Vector{Vector{Float64}}
 @test isapprox( B[1], [10.0;], atol=1e-6)