Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update factors to return residual #397

Merged
merged 1 commit into from
Feb 6, 2021
Merged

Update factors to return residual #397

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 4 additions & 14 deletions src/SensorModels.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,15 +27,8 @@ mutable struct LinearRangeBearingElevation <: IIF.AbstractRelativeMinimize
LinearRangeBearingElevation() = new()
LinearRangeBearingElevation( r::Tuple{Float64,Float64}, b::Tuple{Float64,Float64}; elev=Uniform(-0.25133,0.25133)) = new(Normal(r...),Normal(b...),elev, reuseLBRA[reuseLBRA(0) for i in 1:Threads.nthreads()] )
end
function (cfo::CalcFactor{<:LinearRangeBearingElevation})(res::Vector{Float64},
meas,
pose,
landm )
#
residualLRBE!(res, meas, pose, landm, cfo.factor.reuse[Threads.threadid()])

# residual stored in res
nothing
function (cfo::CalcFactor{<:LinearRangeBearingElevation})(meas, pose, landm)
return residualLRBE!(meas, pose, landm, cfo.factor.reuse[Threads.threadid()])
end

function getSample!(y::Array{Float64,2}, las::LinearRangeBearingElevation, idx::Int )
Expand Down Expand Up @@ -85,8 +78,7 @@ end
# measurement z is measurement vector with [range; bearing; elevation]
# variables are tuple (pose X [dim6], landmark L [dim3])
# function handle follows required parameter list
function residualLRBE!( resid::AbstractVector{<:Real},
z::AbstractVector{<:Real},
function residualLRBE!( z::AbstractVector{<:Real},
X::AbstractVector{<:Real},
L::AbstractVector{<:Real},
reuse::reuseLBRA )
Expand All @@ -95,9 +87,7 @@ function residualLRBE!( resid::AbstractVector{<:Real},
# TODO just switch directly to parameterized function

ominus!(reuse, X, L)
resid .= z .- reuse.rbe

nothing
return z .- reuse.rbe
end


Expand Down
10 changes: 3 additions & 7 deletions src/factors/Bearing2D.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,7 @@ function getSample(cfo::CalcFactor{<:Pose2Point2Bearing}, N::Int=1)
return (reshape(rand(cfo.factor.bearing, N),1,N), )
end
# define the conditional probability constraint
function (cfo::CalcFactor{<:Pose2Point2Bearing})( res::AbstractVector{<:Real},
meas,
xi,
lm )
function (cfo::CalcFactor{<:Pose2Point2Bearing})(meas, xi, lm)
#
reuse = cfo.factor.reuse[Threads.threadid()]
reuse.measvec[1] = cos(meas[1] + xi[3])
Expand All @@ -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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we will probably drop the reuse.residual container here at some point, right?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, there are a couple of different ways in use currently. I agree we can experiment and then standardise.

# reuse.resid .^= 2
# res[1] = reuse.resid[1]
# res[1] += reuse.resid[2]
# return res[1]
nothing

end


Expand Down
67 changes: 15 additions & 52 deletions src/factors/BearingRange2D.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,16 +32,9 @@ function IIF.getParametricMeasurement(s::Pose2Point2BearingRange{<:Normal, <:Nor
end

# TODO consolidate with parametric constraint, follow at #467
function (cfo::CalcFactor{<:Pose2Point2BearingRange})(res::AbstractVector{<:Real},
meas,
xi,
lm )
#
# FIXME, consolidate with parametric IIF #467

function (cfo::CalcFactor{<:Pose2Point2BearingRange})(meas, xi, lm)
# 1-bearing
# 2-range

# world frame
θ = meas[1] + xi[3]
mx = meas[2]*cos(θ)
Expand All @@ -51,57 +44,27 @@ function (cfo::CalcFactor{<:Pose2Point2BearingRange})(res::AbstractVector{<:Real
ey = lm[2] - (my + xi[2])

# res = [eθ, er]
res[1] = atan((my + xi[2]), (mx + xi[1])) - atan(lm[2], lm[1]) # eθ
res[2] = sqrt(ex^2 + ey^2) # some wasted computation here # er

# rot = meas[1]+xi[3]

# res[1] = ( lm[1] - (meas[2]*cos( rot ) + xi[1]) )^2
# res[2] = ( lm[2] - (meas[2]*sin( rot ) + xi[2]) )^2

# res[1] += res[2]
# res[2] = 0.0

# return res[1]

# IIF v0.21+
nothing
eθ = atan((my + xi[2]), (mx + xi[1])) - atan(lm[2], lm[1]) # eθ
er= sqrt(ex^2 + ey^2) # some wasted computation here # er

#Old way
# rot = meas[1]+xi[3]
# res[1] = ( lm[1] - (meas[2]*cos( rot ) + xi[1]) )^2
# res[2] = ( lm[2] - (meas[2]*sin( rot ) + xi[2]) )^2
# res[1] += res[2]
# res[2] = 0.0
# return res[1]

# IIF v0.21+
return [eθ, er]
end

# quick check
# pose = (0,0,0), bear = 0.0, range = 10.0 ==> lm = (10,0)
# pose = (0,0,0), bear = pi/2, range = 10.0 ==> lm = (0,10)
# pose = (0,0,pi/2), bear = 0.0, range = 10.0 ==> lm = (0,10)
# pose = (0,0,pi/2), bear = pi/2, range = 10.0 ==> lm = (-10,0)

#TODO wrapper, consolidate with CalcFactor version, see #467
function (s::Pose2Point2BearingRange{<:Normal})(xi::AbstractVector{T}, lm::AbstractVector{T}; kwargs...) where T <: Real


meas = [mean(s.bearing), mean(s.range)]
iΣ = [1/var(s.bearing) 0;
0 1/var(s.range)]

# 1-bearing
# 2-range

# world frame
θ = meas[1] + xi[3]
mx = meas[2]*cos(θ)
my = meas[2]*sin(θ)

ex = lm[1] - (mx + xi[1])
ey = lm[2] - (my + xi[2])
er = sqrt(ex^2 + ey^2)

eθ = atan((my + xi[2]), (mx + xi[1])) - atan(lm[2], lm[1])

res = [eθ, er]

return res' * iΣ * res

end


Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is great thanks! consolidated.

# Support for database based solving

passTypeThrough(d::FunctionNodeData{Pose2Point2Range}) = d
Expand Down
24 changes: 9 additions & 15 deletions src/factors/DynPoint2D.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,21 +24,16 @@ mutable struct DynPoint2DynPoint2{T <: SamplableBelief} <: AbstractRelativeRoots
end

getSample(cfo::CalcFactor{<:DynPoint2DynPoint2}, N::Int=1) = (rand(cfo.factor.z,N), )
function (cfo::CalcFactor{<:DynPoint2DynPoint2})(
res::AbstractVector{<:Real},
z,
xi,
xj )

function (cfo::CalcFactor{<:DynPoint2DynPoint2})(z, xi, xj)
#
dt = Dates.value(cfo.metadata.fullvariables[2].nstime - cfo.metadata.fullvariables[1].nstime)*1e-9 # roughly the intended use of userdata
res[1:2] = z[1:2] - (xj[1:2] - (xi[1:2]+dt*xi[3:4]))
res[3:4] = z[3:4] - (xj[3:4] - xi[3:4])
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]
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The function will probably get inlined, so it shouldn't make much of a difference. NLSolve reuses the residual in any case.

end




"""
$(TYPEDEF)
"""
Expand All @@ -49,19 +44,18 @@ mutable struct Point2Point2Velocity{T <: IIF.SamplableBelief} <: IIF.AbstractRel
end

getSample(cfo::CalcFactor{<:Point2Point2Velocity}, N::Int=1) = (rand(cfo.factor.z,N), )
function (cfo::CalcFactor{<:Point2Point2Velocity})( res::AbstractVector{<:Real},
z,
function (cfo::CalcFactor{<:Point2Point2Velocity})( z,
xi,
xj )
#
dt = (cfo.metadata.fullvariables[2].nstime - cfo.metadata.fullvariables[1].nstime)*1e-9 # roughly the intended use of userdata
dp = (xj[1:2] .- xi[1:2])
dv = (xj[3:4] .- xi[3:4])

res[1:2] .= z[1:2] .- dp
res[3:4] .= dp/dt .- 0.5*(xj[3:4] .+ xi[3:4]) # (dp/dt - 0.5*(xj[3:4]+xi[3:4])) # midpoint integration
res12 = z[1:2] .- dp
res34 = dp/dt .- 0.5*(xj[3:4] .+ xi[3:4]) # (dp/dt - 0.5*(xj[3:4]+xi[3:4])) # midpoint integration

nothing
return [res12; res34]
end


Expand Down
42 changes: 32 additions & 10 deletions src/factors/DynPose2D.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,31 @@ DynPose2VelocityPrior(z1::T1,z2::T2) where {T1 <: IIF.SamplableBelief, T2 <: IIF
getSample(cf::CalcFactor{<:DynPose2VelocityPrior}, N::Int=1) = ([rand(cf.factor.Zpose,N);rand(cf.factor.Zvel,N)], )


function IIF.getParametricMeasurement(s::DynPose2VelocityPrior{<:MvNormal, <:MvNormal})

meas = [mean(s.Zpose); mean(s.Zvel)]

iΣp = invcov(s.Zpose)
iΣv = invcov(s.Zvel)

iΣ = zeros(eltype(iΣp), 5,5)

iΣ[1:3,1:3] .= iΣp
iΣ[4:5,4:5] .= iΣv

return meas, iΣ
end

function (cf::CalcFactor{<:DynPose2VelocityPrior})(meas, X)
#pose part, reused from PriorPose2
iXihat = SE2(meas[1:3]) \ SE2(X[1:3])
res_pose = se2vee(iXihat)

#velocity part, normal prior
res_vel = meas[4:5] .- X[4:5]

return [res_pose; res_vel]
end

"""
$(TYPEDEF)
Expand All @@ -33,15 +57,14 @@ DynPose2Pose2(z1::T) where {T <: IIF.SamplableBelief} = DynPose2Pose2{T}(z1)

getSample(cf::CalcFactor{<:DynPose2Pose2}, N::Int=1) = (rand(cf.factor.Zpose.z,N), )

function (cf::CalcFactor{<:DynPose2Pose2})( res::Array{Float64},
meas,
function (cf::CalcFactor{<:DynPose2Pose2})( meas,
wXi,
wXj )
#
# cf.factor.Zpose(res, meas, wXi, wXj)
wXjhat = SE2(wXi)*SE2(meas)
jXjhat = SE2(wXj) \ wXjhat
se2vee!(res, jXjhat)
wXjhat = SE2(wXi)*SE2(meas)
jXjhat = SE2(wXj) \ wXjhat
return se2vee(jXjhat)
# z = meas[1][:,idx]
# wxi, wxj = wXi[:,idx], wXj[:,idx]
# dt = (userdata.variableuserdata[2].ut - userdata.variableuserdata[1].ut)*1e-6
Expand Down Expand Up @@ -127,8 +150,7 @@ getManifolds(::DynPose2DynPose2) = getManifolds(DynPose2DynPose2)

getSample(cf::CalcFactor{<:DynPose2DynPose2}, N::Int=1) = (rand(cf.factor.Z, N), )

function (cf::CalcFactor{<:DynPose2DynPose2})(res::AbstractArray{<:Real},
meas,
function (cf::CalcFactor{<:DynPose2DynPose2})(meas,
wXi,
wXj )
#
Expand All @@ -141,10 +163,10 @@ function (cf::CalcFactor{<:DynPose2DynPose2})(res::AbstractArray{<:Real},
dt = Dates.value(cf.metadata.fullvariables[2].nstime - cf.metadata.fullvariables[1].nstime)*1e-9
wpj = ( wxi[1:2]+dt*wxi[4:5] + z[1:2] )
thetaj = se2vee(SE2([0;0;wxi[3]])*SE2([0;0;z[3]]))[3]
res[1:3] = se2vee( SE2(wxj[1:3])\SE2([wpj;thetaj]) )
res13 = se2vee( SE2(wxj[1:3])\SE2([wpj;thetaj]) )
# res[1:2] = (wXj[1:2] - (wXi[1:2]+dt*wXi[4:5])+z[1:2])
res[4:5] = z[4:5] - (wxj[4:5] - wxi[4:5])
nothing
res45 = z[4:5] - (wxj[4:5] - wxi[4:5])
return [res13;res45]
end


Expand Down
9 changes: 2 additions & 7 deletions src/factors/MutablePose2Pose2.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,16 +26,11 @@ Related

Pose2Pose2, Pose3Pose3, InertialPose3, DynPose2Pose2, Point2Point2, VelPoint2VelPoint2
"""
function (cfo::CalcFactor{<:MutablePose2Pose2Gaussian})(
res::AbstractVector{<:Real},
meas,
wxi,
wxj )
function (cfo::CalcFactor{<:MutablePose2Pose2Gaussian})(meas, wxi, wxj)
#
wXjhat = SE2(wxi[1:3])*SE2(meas[1:3])
jXjhat = SE2(wxj[1:3]) \ wXjhat
se2vee!(res, jXjhat)
nothing
return se2vee(jXjhat)
end


Expand Down
8 changes: 2 additions & 6 deletions src/factors/PartialPose3.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -150,17 +150,13 @@ PartialPose3XYYaw(xy::T1, yaw::T2) where {T1 <: IIF.SamplableBelief, T2 <: IIF.S
function getSample(cfo::CalcFactor{<:PartialPose3XYYaw}, N::Int=1)
return ([rand(cfo.factor.xy,N);rand(cfo.factor.yaw,N)[:]'], )
end
function (cfo::CalcFactor{<:PartialPose3XYYaw})(res::AbstractVector{<:Real},
meas,
function (cfo::CalcFactor{<:PartialPose3XYYaw})(meas,
wXi,
wXj )
#
wXjhat = SE2(wXi[[1;2;6]]) * SE2(meas[1:3])
jXjhat = SE2(wXj[[1;2;6]]) \ wXjhat
se2vee!(res, jXjhat)

# res'*res
nothing
return se2vee(jXjhat)
end

"""
Expand Down
33 changes: 4 additions & 29 deletions src/factors/Point2D.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,23 +18,10 @@ function getSample(cfo::CalcFactor{<:PriorPoint2}, N::Int=1)
return (rand(cfo.factor.Z, N),)
end

function (s::CalcFactor{<:PriorPoint2})(res::AbstractVector{<:Real},
meas,
function (s::CalcFactor{<:PriorPoint2})(meas,
X1)
#
res[1:2] .= meas[1:2] .- X1[1:2]

nothing
end

#TODO wrapper
function (s::PriorPoint2{<:MvNormal})(X1::AbstractVector{T}; kwargs...) where T <: Real

meas = mean(s.Z)
iΣ = invcov(s.Z)
res = meas[1:2] .- X1[1:2]
return res' * iΣ * res

return meas[1:2] .- X1[1:2]
end

"""
Expand All @@ -53,25 +40,13 @@ Point2Point2(x::T=MvNormal(zeros(2),LinearAlgebra.diagm([0.1;0.1]))) where {T <:
function getSample(cfo::CalcFactor{<:Point2Point2}, N::Int=1)
return (rand(cfo.factor.Zij,N), )
end
function (pp2r::CalcFactor{<:Point2Point2})(res::AbstractVector{<:Real},
meas,
function (pp2r::CalcFactor{<:Point2Point2})(meas,
xi,
xj )
#
res[1:2] .= meas[1:2] .- (xj[1:2] .- xi[1:2])
nothing
return meas[1:2] .- (xj[1:2] .- xi[1:2])
end

#TODO wrapper, consolidate see #467
function (s::Point2Point2{<:MvNormal})(X1::AbstractVector{T}, X2::AbstractVector{T}; kwargs...) where T <: Real

meas = mean(s.Zij)
iΣ = invcov(s.Zij)
res = meas[1:2] .- (X2[1:2] .- X1[1:2])
return res' * iΣ * res
end





Expand Down
Loading