Merge pull request #1333 from JuliaRobotics/21Q3/api/calcfactresid

swap arg order calcFactorRe.. and _evalFactorTemp...

Author: dehann

src/ApproxConv.jl

@@ -409,8 +409,9 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
   ipc = if ccwl._gradients === nothing 
     ones(getDimension(Xi[sfidx]))
   else
-    # calcPerturbationFromVariable(ccwl._gradients, 2, ipc_)
-    ones(getDimension(Xi[sfidx]))
+    ipc_ = ones(getDimension(Xi[sfidx]))
+    # calcPerturbationFromVariable(ccwl._gradients, 2, ipc_) # TODO, WIP
+    ipc_                                                     # TODO, WIP
   end
 
   # return the found points, and info per coord
@@ -597,7 +598,7 @@ Notes
 
 Example
 ```julia
-B = _evalFactorTemporary!(EuclidDistance, ([10;],), 2, [(ContinuousScalar,[0]); (ContinuousScalar,[9.5;])] )
+B = _evalFactorTemporary!(EuclidDistance, (ContinuousScalar, ContinuousScalar), 2, ([10;],), ([0.],[9.5]) )
 # should return `B = 10`
 ```
 
@@ -606,9 +607,9 @@ Related
 [`evalFactor`](@ref), [`calcFactorResidual`](@ref), [`testFactorResidualBinary`](@ref)
 """
 function _evalFactorTemporary!( fct::AbstractFactor,
+                                varTypes::Tuple,
                                 sfidx::Int,  # solve for index, assuming variable order for fct
                                 meas_single::Tuple,
-                                varTypes::Tuple,
                                 pts::Tuple;
                                 tfg::AbstractDFG=initfg(),
                                 solveKey::Symbol=:default,

src/CalcFactor.jl

@@ -103,16 +103,16 @@ DevNotes
 
 Example
 ```julia
-residual = calcFactorResidualTemporary(Pose2Pose2(...), (z_i,), (RoME.Pose2, x1), (RoME.Pose2, x2))
+residual = calcFactorResidualTemporary(Pose2Pose2(...), (RoME.Pose2,RoME.Pose2), (z_i,), (x1, x2))
 ```
 
 Related
 
 [`calcFactorResidual`](@ref), [`CalcResidual`](@ref), [`_evalFactorTemporary!`](@ref), [`approxConv`](@ref), [`_buildGraphByFactorAndTypes!`](@ref)
 """
 function calcFactorResidualTemporary( fct::AbstractRelative, 
-                                      measurement::Tuple,
                                       varTypes::Tuple,
+                                      measurement::Tuple,
                                       pts::Tuple;
                                       tfg::AbstractDFG = initfg(),
                                       _blockRecursion::Bool=false )

src/Deprecated.jl

@@ -59,7 +59,7 @@ end
 #   return msst_
 # end
 
-@deprecate testFactorResidualBinary(fct, T1::InstanceType{InferenceVariable}, T2::InstanceType{InferenceVariable}, param1, param2, meas::Tuple) calcFactorResidualTemporary(fct, meas, (T1,param1), (T2,param2))
+@deprecate testFactorResidualBinary(fct, T1::InstanceType{InferenceVariable}, T2::InstanceType{InferenceVariable}, param1, param2, meas::Tuple) calcFactorResidualTemporary(fct, (T1,T2), meas, (param1,param2))
 
 # """
 #     $SIGNATURES

src/FactorGraph.jl

@@ -680,10 +680,10 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
   gradients = nothing
   # prepare new cached gradient lambdas (attempt)
   try
-    # this try block definitely fails on deserialization, due to empty DFGVariable[] vector here:
     # https://github.com/JuliaRobotics/IncrementalInference.jl/blob/db7ff84225cc848c325e57b5fb9d0d85cb6c79b8/src/DispatchPackedConversions.jl#L46
     # also https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/590#issuecomment-891450762
-    if (!_blockRecursion) && usrfnc isa AbstractRelative
+    # FIXME, suppressing nested gradient propagation on GenericMarginals for the time being, see #1010
+    if (!_blockRecursion) && usrfnc isa AbstractRelative && !(usrfnc isa GenericMarginal)
       # take first value from each measurement-tuple-element
       measurement_ = map(x->x[1], meas_single)
       # compensate if no info available during deserialization

src/services/FactorGradients.jl

@@ -13,7 +13,7 @@ function _prepFactorGradientLambdas(fct::Union{<:AbstractRelativeMinimize,<:Abst
                                     tfg::AbstractDFG = initfg(),
                                     _blockRecursion::Bool=true,
                                     # gradients relative to coords requires 
-                                    slack_resid = calcFactorResidualTemporary(fct, measurement, varTypes, pts, tfg=tfg, _blockRecursion=_blockRecursion),
+                                    slack_resid = calcFactorResidualTemporary(fct, varTypes, measurement, pts, tfg=tfg, _blockRecursion=_blockRecursion),
                                     # numerical diff perturbation size
                                     h::Real=1e-4  ) 
   #
@@ -29,7 +29,7 @@ function _prepFactorGradientLambdas(fct::Union{<:AbstractRelativeMinimize,<:Abst
   T_pth_s_i =     (s,i) -> AMP.makePointFromCoords(M[s], coord_h(s,i), pts[s])         # exp(M,..., hat(M,...) )
   tup_pt_s_i_h =  (s,i) -> tuple(pts[1:(s-1)]..., T_pth_s_i(s,i), pts[(s+1):end]...)
   # build a residual calculation specifically considering graph factor selections `s`, e.g. for binary `s ∈ {1,2}`.
-  f_dsi_h =       (d,s,i) -> IIF._evalFactorTemporary!(fct, d, measurement, varTypes, tup_pt_s_i_h(s,i); tfg=tfg, newFactor=false, currNumber=0, _slack=slack_resid )
+  f_dsi_h =       (d,s,i) -> IIF._evalFactorTemporary!(fct, varTypes, d, measurement, tup_pt_s_i_h(s,i); tfg=tfg, newFactor=false, currNumber=0, _slack=slack_resid )
   # standard calculus derivative definition (in coordinate space)
   Δf_dsi =        (d,s,i, crd=coord_(d)) -> (f_dsi_h(d,s,i)[1] - crd)./h
   # jacobian block per s, for each i
@@ -129,7 +129,7 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
   measurement = tuple(meas_pts[1:lenm]...)
   pts = tuple(meas_pts[(1+lenm):end]...)
   varTypes = tuple(getVariableType.(getVariable.(fgc._tfg, getVariableOrder(fgc.dfgfct)))...)
-  new_slack = calcFactorResidualTemporary(fct, measurement, varTypes, pts; tfg=fgc._tfg)
+  new_slack = calcFactorResidualTemporary(fct, varTypes, measurement, pts; tfg=fgc._tfg)
   # TODO make sure slack_residual is properly wired up with all the lambda functions as expected
   _setFGCSlack!(fgc, new_slack)
   # _setPointsMani!(fgc.slack_residual, new_slack)

test/testApproxConv.jl

@@ -23,9 +23,10 @@ ccw = IIF._getCCW(fg, :x0x1f1)
 
 @test IIF._getZDim(ccw) == 3
 
-res = calcFactorResidualTemporary(lr3, ([0;0;0.5],),
+res = calcFactorResidualTemporary(lr3,
                                   (ContinuousEuclid{3},ContinuousEuclid{3}),
-                                  (zeros(3), [0;0;1.0]) )
+                                  ([0;0;0.5],),
+                                  (zeros(3), [0;0;1.0])  )
 #
 
 @test (sum(abs.(res)) - 0.5) < 1e-10

test/testGradientUtils.jl

@@ -20,7 +20,7 @@ dfg, _dfgfct = IIF._buildGraphByFactorAndTypes!(fct, varTypes, varPts)
 
 ## test  the evaluation of factor without
 
-B = IIF._evalFactorTemporary!(EuclidDistance(Normal(10,1)), 2, ([10;],), varTypes, varPts );
+B = IIF._evalFactorTemporary!(EuclidDistance(Normal(10,1)), varTypes, 2, ([10;],), varPts );
 
 @test_broken B isa Vector{Vector{Float64}}
 @test isapprox( B[1], [10.0;], atol=1e-6)
@@ -39,25 +39,25 @@ pts = ([0;],[9.5;])
 
 ##
 
-slack_resid = calcFactorResidualTemporary(fct, measurement, varTypes, pts)
+slack_resid = calcFactorResidualTemporary(fct, varTypes, measurement, pts)
 
 ##
 
-coord_1 = IIF._evalFactorTemporary!(fct, 1, measurement, varTypes, pts, _slack=slack_resid )
+coord_1 = IIF._evalFactorTemporary!(fct, varTypes, 1, measurement, pts, _slack=slack_resid )
 @test length(coord_1) == 1
 @test isapprox( coord_1[1], [0.0], atol=1e-6)
 
-coord_2 = IIF._evalFactorTemporary!(fct, 2, measurement, varTypes, pts, _slack=slack_resid )
+coord_2 = IIF._evalFactorTemporary!(fct, varTypes, 2, measurement, pts, _slack=slack_resid )
 @test length(coord_2) == 1
 @test isapprox( coord_2[1], [9.5], atol=1e-6)
 
 ##
 
-coord_1 = IIF._evalFactorTemporary!(fct, 1, measurement, varTypes, pts )
+coord_1 = IIF._evalFactorTemporary!(fct, varTypes, 1, measurement, pts )
 @test length(coord_1) == 1
 @test isapprox( coord_1[1], [-0.5], atol=1e-6)
 
-coord_2 = IIF._evalFactorTemporary!(fct, 2, measurement, varTypes, pts )
+coord_2 = IIF._evalFactorTemporary!(fct, varTypes, 2, measurement, pts )
 @test length(coord_2) == 1
 @test isapprox( coord_2[1], [10.0], atol=1e-6)
 
@@ -89,7 +89,7 @@ _fg,_ = IIF._buildGraphByFactorAndTypes!(fct, varTypes, pts);
 
 _fg = initfg()
 
-slack_resid = calcFactorResidualTemporary(fct, measurement, varTypes, pts, tfg=_fg)
+slack_resid = calcFactorResidualTemporary(fct, varTypes, measurement, pts, tfg=_fg)
 
 @test length(getVal(_fg[:x1])) == 1
 @test length(getVal(_fg[:x1])[1]) == 2
@@ -100,7 +100,7 @@ slack_resid = calcFactorResidualTemporary(fct, measurement, varTypes, pts, tfg=_
 ## Manually provide a common temp graph and force no factor and same variables via keywords
 
 tfg,_ = IIF._buildGraphByFactorAndTypes!(fct, varTypes, pts);
-coord_1 = IIF._evalFactorTemporary!(fct, 1, measurement, varTypes, pts, _slack=slack_resid, tfg=tfg, newFactor=false, currNumber=0 )
+coord_1 = IIF._evalFactorTemporary!(fct, varTypes, 1, measurement, pts, _slack=slack_resid, tfg=tfg, newFactor=false, currNumber=0 )
 
 ##
 
@@ -109,23 +109,23 @@ coord_1 = IIF._evalFactorTemporary!(fct, 1, measurement, varTypes, pts, _slack=s
 
 @test isapprox( coord_1[1], [0;0.0], atol=1e-6)
 
-coord_2 = IIF._evalFactorTemporary!(fct, 2, measurement, varTypes, pts, _slack=slack_resid )
+coord_2 = IIF._evalFactorTemporary!(fct, varTypes, 2, measurement, pts, _slack=slack_resid )
 @test length(coord_2) == 1
 @test length(coord_2[1]) == 2
 
 @test isapprox( coord_2[1], [9.5; 0], atol=1e-6)
 
 ## Repeat the same test but allow _evalFactorTemporary to self construct internal temporary graph
 
-coord_1 = IIF._evalFactorTemporary!(fct, 1, measurement, varTypes, pts )
+coord_1 = IIF._evalFactorTemporary!(fct, varTypes, 1, measurement, pts )
 
 ##
 @test length(coord_1) == 1
 @test length(coord_1[1]) == 2
 
 @test isapprox( coord_1[1], [-0.5;0], atol=1e-6)
 
-coord_2 = IIF._evalFactorTemporary!(fct, 2, measurement, varTypes, pts )
+coord_2 = IIF._evalFactorTemporary!(fct, varTypes, 2, measurement, pts )
 @test length(coord_2) == 1
 @test length(coord_2[1]) == 2
 

test/testpartialconstraint.jl

@@ -284,7 +284,7 @@ val2_ = getVal(v1)
 val_, = predictbelief(fg, v2, [f3;f4], N=N)
 @cast val[i,j] := val_[j][i]
 # plotKDE(kde!(val),levels=3)
-@test_broken norm(Statistics.mean(val,dims=2)[1] .- [-20.0]) < 0.01
+@test norm(Statistics.mean(val,dims=2)[1] .- [-20.0]) < 1
 @test_broken norm(Statistics.mean(val,dims=2)[2] .- [10.0]) < 0.01
 @test (Statistics.std(val,dims=2)[1] .- 1.0) < 3.0
 @test (Statistics.std(val,dims=2)[2] .- 1.0) < 3.0