Update factors to return residual #397
Conversation
There was a problem hiding this comment.
all good thanks! Let's tag so long but then experiment with different ways to get the right type on cfo.residual? The user can then find a memory allocation problem in factor function and hook into the (latest possible "internally" pre-allocated) cfo.residual container?
| @@ -40,12 +37,11 @@ function (cfo::CalcFactor{<:Pose2Point2Bearing})( res::AbstractVector{<:Real}, | |||
| reuse.resid .-= reuse.predvec | |||
|
|
|||
| # must return result of length zDim==1 in this case | |||
| res[1] = sum(abs.(reuse.resid)) | |||
| return sum(abs.(reuse.resid)) | |||
There was a problem hiding this comment.
we will probably drop the reuse.residual container here at some point, right?
There was a problem hiding this comment.
Yes, there are a couple of different ways in use currently. I agree we can experiment and then standardise.
|
|
||
| end | ||
|
|
||
|
|
There was a problem hiding this comment.
this is great thanks! consolidated.
| nothing | ||
| res12 = z[1:2] - (xj[1:2] - (xi[1:2]+dt*xi[3:4])) | ||
| res34 = z[3:4] - (xj[3:4] - xi[3:4]) | ||
| return [res12; res34] |
There was a problem hiding this comment.
we'll see how the allocations go. if bad then we can turn to using the cfo.residual container -- likely best done after consolidating CPT?
There was a problem hiding this comment.
The function will probably get inlined, so it shouldn't make much of a difference. NLSolve reuses the residual in any case.
| # | ||
| XX = lm[1] - (rho[1]*cos(theta[1]) + xi[1]) | ||
| YY = lm[2] - (rho[1]*sin(theta[1]) + xi[2]) | ||
| res[1] = XX^2 + YY^2 | ||
| return nothing | ||
| #TODO JT - Should this have a sqrt for parametric? |
There was a problem hiding this comment.
my feeling is yes, else the Malahanobis distance will be wrong. else factors will be mixing ^2 and ^4 costs. sort is a little excess but safest path for now i think
There was a problem hiding this comment.
See this comparison from slack: "Actually the getSample theta part (2pirand(N)) won't work in any case"
https://github.com/JuliaRobotics/RoME.jl/compare/feat/21Q1/factor_residuals...twig/21Q1/parametricPoint2Point2Range?expand=1
| Xi, | ||
| Xj ) | ||
| # | ||
| #FIXME JT - Createing new res for simplicity, it may not hold up well though | ||
| res = Vector{eltype(Xi)}(undef, 5) |
There was a problem hiding this comment.
ah cool okay, let's try a few ways and that will hopefully help show how to do it natively in cfo.residual for all factors, available if users want to use it to reduce allocations. Think it's okay to experiment with this for a bit
| f = Pose2Point2BearingRange(Normal(-pi/2,1), Normal(10.0,1)) | ||
| @test isapprox(f([0.,0,0], [0.,-10]), 0, atol = 1e-9) | ||
| @test isapprox(f([0,0,pi/2], [10.,0]), 0, atol = 1e-9) | ||
| # #TODO Update to new CalcFactor |
There was a problem hiding this comment.
yeah, i've been trying to find a nice API in IIf for these tests. I basically start with IIF.testResidualBinary (or near about)
| ensureAllInitialized!(fg) | ||
|
|
||
| vardict, result, varIds, Σ = IIF.solveFactorGraphParametric!(fg) |
There was a problem hiding this comment.
It just takes some time to update and test, but most factors should work with AD without much effort.
No description provided.