From b3eb5c43e3a55455120ccc86ac1a173c329c6b28 Mon Sep 17 00:00:00 2001 From: Johan Terblanche <6612981+Affie@users.noreply.github.com> Date: Sat, 6 Feb 2021 15:38:12 +0200 Subject: [PATCH] Update factors to return residual --- src/SensorModels.jl | 18 ++------- src/factors/Bearing2D.jl | 10 ++--- src/factors/BearingRange2D.jl | 67 +++++++------------------------- src/factors/DynPoint2D.jl | 24 +++++------- src/factors/DynPose2D.jl | 42 +++++++++++++++----- src/factors/MutablePose2Pose2.jl | 9 +---- src/factors/PartialPose3.jl | 8 +--- src/factors/Point2D.jl | 33 ++-------------- src/factors/Pose2D.jl | 21 +--------- src/factors/Pose2Point2.jl | 11 +----- src/factors/Pose3Pose3.jl | 10 ++--- src/factors/PriorPose2.jl | 24 +----------- src/factors/Range2D.jl | 19 +++------ src/factors/VelPoint2D.jl | 13 ++++--- src/factors/VelPose2D.jl | 33 +++++++++++++--- test/testBearingRange2D.jl | 34 ++++++++-------- test/testParametric.jl | 31 ++++++++++++++- test/testhigherdimroots.jl | 8 ++-- 18 files changed, 173 insertions(+), 242 deletions(-) diff --git a/src/SensorModels.jl b/src/SensorModels.jl index e352f7be..f3c5c77a 100644 --- a/src/SensorModels.jl +++ b/src/SensorModels.jl @@ -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 ) @@ -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 ) @@ -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 diff --git a/src/factors/Bearing2D.jl b/src/factors/Bearing2D.jl index 8ab9e666..17e9319b 100644 --- a/src/factors/Bearing2D.jl +++ b/src/factors/Bearing2D.jl @@ -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]) @@ -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)) # reuse.resid .^= 2 # res[1] = reuse.resid[1] # res[1] += reuse.resid[2] - # return res[1] - nothing + end diff --git a/src/factors/BearingRange2D.jl b/src/factors/BearingRange2D.jl index f88257d7..49875a38 100644 --- a/src/factors/BearingRange2D.jl +++ b/src/factors/BearingRange2D.jl @@ -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(θ) @@ -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 - - # Support for database based solving passTypeThrough(d::FunctionNodeData{Pose2Point2Range}) = d diff --git a/src/factors/DynPoint2D.jl b/src/factors/DynPoint2D.jl index f0fe8bd0..cc953fc0 100644 --- a/src/factors/DynPoint2D.jl +++ b/src/factors/DynPoint2D.jl @@ -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] end - - """ $(TYPEDEF) """ @@ -49,8 +44,7 @@ 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 ) # @@ -58,10 +52,10 @@ function (cfo::CalcFactor{<:Point2Point2Velocity})( res::AbstractVector{<:Real}, 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 diff --git a/src/factors/DynPose2D.jl b/src/factors/DynPose2D.jl index 71ea55f5..33fb3afe 100644 --- a/src/factors/DynPose2D.jl +++ b/src/factors/DynPose2D.jl @@ -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) @@ -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 @@ -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 ) # @@ -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 diff --git a/src/factors/MutablePose2Pose2.jl b/src/factors/MutablePose2Pose2.jl index 80fa5b9b..f227bf92 100644 --- a/src/factors/MutablePose2Pose2.jl +++ b/src/factors/MutablePose2Pose2.jl @@ -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 diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index 2f4961e9..5cb0ea98 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -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 """ diff --git a/src/factors/Point2D.jl b/src/factors/Point2D.jl index eaee118d..da66d554 100644 --- a/src/factors/Point2D.jl +++ b/src/factors/Point2D.jl @@ -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 """ @@ -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 - - diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index f6f955e5..440d6b6c 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -21,33 +21,16 @@ Pose2Pose2(::UniformScaling) = Pose2Pose2(MvNormal(zeros(3),LinearAlgebra.diagm( getSample(cf::CalcFactor{<:Pose2Pose2}, N::Int=1) = (rand(cf.factor.z,N), ) -function (cf::CalcFactor{<:Pose2Pose2})(res::AbstractVector{<:Real}, - meas, +function (cf::CalcFactor{<:Pose2Pose2})(meas, wxi, wxj ) # wXjhat = SE2(wxi)*SE2(meas) jXjhat = SE2(wxj) \ wXjhat - se2vee!(res, jXjhat) - nothing + return se2vee(jXjhat) end -#TODO wrapper -- consolidate with CalcFactor, see #467 -function (s::Pose2Pose2{<:MvNormal})(wXi::AbstractVector{T}, wXj::AbstractVector{T}; kwargs...) where T <: Real - - meas = mean(s.z) - iΣ = invcov(s.z) - wXjhat = SE2(wXi[1:3])*SE2(meas[1:3]) - jXjhat = SE2(wXj[1:3]) \ wXjhat - - res = se2vee(jXjhat) - return res' * iΣ * res - -end - - - # NOTE, serialization support -- will be reduced to macro in future # ------------------------------------ diff --git a/src/factors/Pose2Point2.jl b/src/factors/Pose2Point2.jl index 770098e5..44a7cf58 100644 --- a/src/factors/Pose2Point2.jl +++ b/src/factors/Pose2Point2.jl @@ -34,20 +34,13 @@ function IIF.getParametricMeasurement(s::Pose2Point2{<:MvNormal}) end # define the conditional probability constraint -function (cfo::CalcFactor{<:Pose2Point2})(res::AbstractVector{<:Real}, - meas, +function (cfo::CalcFactor{<:Pose2Point2})(meas, wXi, wLj ) # wLj_pred = SE2(wXi)*SE2([meas;0.0]) - res[1:2] .= wLj .- se2vee(wLj_pred)[1:2] - - # res .^= 2 - # res[1] += res[2] - # res[2] = 0.0 - # return res[1] - nothing + return wLj .- se2vee(wLj_pred)[1:2] end diff --git a/src/factors/Pose3Pose3.jl b/src/factors/Pose3Pose3.jl index 58dd3924..13f70c20 100644 --- a/src/factors/Pose3Pose3.jl +++ b/src/factors/Pose3Pose3.jl @@ -15,7 +15,6 @@ mutable struct PP3REUSE end function fastpose3pose3residual!( reusethrid::PP3REUSE, - res::AbstractVector{<:Real}, meas, wXi, wXj ) @@ -29,8 +28,7 @@ function fastpose3pose3residual!( reusethrid::PP3REUSE, jTi = SE3( matrix(reusethrid.wTj)\matrix(reusethrid.wTi) ) # also wasted memory here, should operate directly on iTi and not be assigning new memory reusethrid.iTi = (SE3(meas[1:3],Euler(meas[4:6]...)) * jTi) - res[:] = veeEuler(reusethrid.iTi) - nothing + return veeEuler(reusethrid.iTi) end """ @@ -49,14 +47,12 @@ Pose3Pose3(z::T=MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones( function getSample(cf::CalcFactor{<:Pose3Pose3}, N::Int=1) return (rand(cf.factor.Zij, N), ) end -function (cf::CalcFactor{<:Pose3Pose3})(res::AbstractVector{<:Real}, - meas, +function (cf::CalcFactor{<:Pose3Pose3})(meas, wXi, wXj ) # reusethrid = cf.factor.reuse[Threads.threadid()] - fastpose3pose3residual!(reusethrid, res, meas, wXi, wXj) - nothing + return fastpose3pose3residual!(reusethrid, meas, wXi, wXj) end """ diff --git a/src/factors/PriorPose2.jl b/src/factors/PriorPose2.jl index eb028d9a..fa2fa31a 100644 --- a/src/factors/PriorPose2.jl +++ b/src/factors/PriorPose2.jl @@ -24,33 +24,13 @@ function getSample(cfo::CalcFactor{<:PriorPose2}, N::Int=1) return (rand(cfo.factor.Z,N), ) end -function (s::CalcFactor{<:PriorPose2})(res::AbstractVector{<:Real}, - meas, +function (s::CalcFactor{<:PriorPose2})(meas, wXi) # iXihat = SE2(meas) \ SE2(wXi) - #NOTE do not forget the .= - res .= se2vee(iXihat) - - nothing + return se2vee(iXihat) end -#TODO wrapper Consolidate with CalcFactor version, see #467 -function (s::PriorPose2{<:MvNormal})(wXi::AbstractVector{<:Real}; kwargs...) - - meas = mean(s.Z) - iΣ = invcov(s.Z) - - # res = meas .- X1 - iXihat = SE2(meas[1:3]) \ SE2(wXi[1:3]) - res = se2vee(iXihat) - - return res' * iΣ * res -end - - - - ## Serialization support """ diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl index c863cd34..e567da11 100644 --- a/src/factors/Range2D.jl +++ b/src/factors/Range2D.jl @@ -13,17 +13,13 @@ Point2Point2Range(d::D) where {D <: IIF.SamplableBelief} = Point2Point2Range{D}( function getSample(cfo::CalcFactor{<:Point2Point2Range}, N::Int=1) return (reshape(rand(cfo.factor.Z,N),1,N), 2*pi*rand(N)) end -function (cfo::CalcFactor{<:Point2Point2Range})( - res::AbstractVector{<:Real}, - rho, - theta, - xi, - lm ) +function (cfo::CalcFactor{<:Point2Point2Range})(rho, theta, xi, lm) # 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? + # return XX^2 + YY^2 + return sqrt(XX^2 + YY^2) end @@ -62,8 +58,7 @@ Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range{T}(Z) function getSample(cfo::CalcFactor{<:Pose2Point2Range}, N::Int=1) return (reshape(rand(cfo.factor.Z,N),1,N) , 2*pi*rand(N)) end -function (pp2r::CalcFactor{<:Pose2Point2Range})(res::AbstractVector{<:Real}, - rho, +function (pp2r::CalcFactor{<:Pose2Point2Range})(rho, theta, xi, lm ) @@ -72,9 +67,7 @@ function (pp2r::CalcFactor{<:Pose2Point2Range})(res::AbstractVector{<:Real}, # this is the noisy range XX = lm[1] - (rho[1]*cos(theta[1]) + xi[1]) YY = lm[2] - (rho[1]*sin(theta[1]) + xi[2]) - res[1] = sqrt(XX^2 + YY^2) - - nothing + return sqrt(XX^2 + YY^2) end diff --git a/src/factors/VelPoint2D.jl b/src/factors/VelPoint2D.jl index 95d6f9a5..eff5a086 100644 --- a/src/factors/VelPoint2D.jl +++ b/src/factors/VelPoint2D.jl @@ -15,11 +15,11 @@ VelPoint2VelPoint2(z1::T) where {T <: Distribution} = VelPoint2VelPoint2{T}(z1) getSample(cfo::CalcFactor{<:VelPoint2VelPoint2}, N::Int=1) = (rand(cfo.factor.z,N), ) -function (cfo::CalcFactor{<:VelPoint2VelPoint2})( res::AbstractVector{<:Real}, - z, - xi, - xj ) +function (cfo::CalcFactor{<:VelPoint2VelPoint2})(z, xi, xj) # + #FIXME JT - I'm createing new res for simplicity, it may not hold up well though + res = Vector{eltype(xi)}(undef, 4) + # change in time from microseconds with DynPoint2(ut=1_000_000) to seconds dt = Dates.value(cfo.metadata.fullvariables[2].nstime - cfo.metadata.fullvariables[1].nstime)*1e-9 # roughly the intended use of userdata # change in psoition Xi \ Xj @@ -37,6 +37,7 @@ function (cfo::CalcFactor{<:VelPoint2VelPoint2})( res::AbstractVector{<:Real}, res[3:4] .+= (dp_dt - xi[3:4]).^2 # (meas - predicted) velocity error term res[3:4] .= sqrt.(res[3:4]) + return res # res[1] = 0.0 # res[1] += sum((z[1:2] - dp).^2) # (meas - predicted) change in position error term # res[1] += sum((z[3:4] - dv).^2) # (meas - predicted) change in velocity error term @@ -53,8 +54,8 @@ function (cfo::CalcFactor{<:VelPoint2VelPoint2})( res::AbstractVector{<:Real}, # return objective cost < IIF v0.21 # return res[1] - # IIF v0.12+ - nothing + # IIF v0.21+ + # return residual end diff --git a/src/factors/VelPose2D.jl b/src/factors/VelPose2D.jl index 5e616b09..5ce9a9a2 100644 --- a/src/factors/VelPose2D.jl +++ b/src/factors/VelPose2D.jl @@ -16,11 +16,28 @@ VelPose2VelPose2(z1::T1, z2::T2) where {T1 <: IIF.SamplableBelief, T2 <: IIF.Sam getSample(cf::CalcFactor{<:VelPose2VelPose2}, N::Int=1) = ([rand(cf.factor.Zpose.z,N);rand(cf.factor.Zvel,N)], ) -function (cf::CalcFactor{<:VelPose2VelPose2})(res::AbstractVector{<:Real}, - meas, +function IIF.getParametricMeasurement(s::VelPose2VelPose2{<:MvNormal, <:MvNormal}) + + meas = [mean(s.Zpose.z); mean(s.Zvel)] + + iΣp = invcov(s.Zpose.z) + 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{<:VelPose2VelPose2})(meas, Xi, Xj ) # + #FIXME JT - Createing new res for simplicity, it may not hold up well though + res = Vector{eltype(Xi)}(undef, 5) + z = meas wxi, wxj = Xi, Xj # @show z, wxi, wxj @@ -28,7 +45,12 @@ function (cf::CalcFactor{<:VelPose2VelPose2})(res::AbstractVector{<:Real}, # fill!(vp2vp2.reuseres[Threads.threadid()], 0.0) wXjhat = SE2(wxi[1:3])*SE2(meas[1:3]) jXjhat = SE2(wxj[1:3]) \ wXjhat - se2vee!(cf.factor.reuseres[Threads.threadid()], jXjhat) + + #FIXME this does not work with parametric + # se2vee!(cf.factor.reuseres[Threads.threadid()], jXjhat) + #FIXME cf.factor.reuseres has type issues with parametric + se2vee!(res, jXjhat) + # vp2vp2.Zpose(vp2vp2.reuseres[Threads.threadid()], userdata, idx, meas, Xi, Xj) wDXij = (wxj[4:5]-wxi[4:5]) @@ -36,7 +58,8 @@ function (cf::CalcFactor{<:VelPose2VelPose2})(res::AbstractVector{<:Real}, # calculate the residual dx = se2vee(SE2(wxi[1:3]) \ SE2(wxj[1:3])) - res[1:3] .= cf.factor.reuseres[Threads.threadid()] + #FIXME cf.factor.reuseres has type issues with parametric + # res[1:3] .= cf.factor.reuseres[Threads.threadid()] res[4:5] .= z[4:5] .- bDXij res[4:5] .^= 2 res[4:5] .+= (dx[1:2]/dt .- 0.5*(wxj[4:5] .+ wxi[4:5])).^2 @@ -47,7 +70,7 @@ function (cf::CalcFactor{<:VelPose2VelPose2})(res::AbstractVector{<:Real}, # res[1] += sum((dx[1:2]/dt - 0.5*(wxj[4:5]+wxi[4:5])).^2) # first order integration # res[1] - nothing + return res end diff --git a/test/testBearingRange2D.jl b/test/testBearingRange2D.jl index 7d677b73..93a1758f 100644 --- a/test/testBearingRange2D.jl +++ b/test/testBearingRange2D.jl @@ -122,23 +122,23 @@ res = testFactorResidualBinary( p2br, @test norm(res) < 1e-14 ## -#TODO Update to new CalcFactor -# test parametric Pose2Point2BearingRange -f = Pose2Point2BearingRange(Normal(0.0,1), Normal(10.0,1)) -@test isapprox(f([0.,0,0], [10.,0]), 0, atol = 1e-9) -@test isapprox(f([0,0,pi/2], [0.,10]), 0, atol = 1e-9) - -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) - -f = Pose2Point2BearingRange(Normal(pi,1), Normal(10.0,1)) -@test isapprox(f([0.,0,0], [-10.,0]), 0, atol = 1e-9) -@test isapprox(f([0,0,pi/2], [0.,-10]), 0, atol = 1e-9) - -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 +# # test parametric Pose2Point2BearingRange +# f = Pose2Point2BearingRange(Normal(0.0,1), Normal(10.0,1)) +# @test isapprox(f([0.,0,0], [10.,0]), 0, atol = 1e-9) +# @test isapprox(f([0,0,pi/2], [0.,10]), 0, atol = 1e-9) + +# 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) + +# f = Pose2Point2BearingRange(Normal(pi,1), Normal(10.0,1)) +# @test isapprox(f([0.,0,0], [-10.,0]), 0, atol = 1e-9) +# @test isapprox(f([0,0,pi/2], [0.,-10]), 0, atol = 1e-9) + +# 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) end diff --git a/test/testParametric.jl b/test/testParametric.jl index 38a4e1a1..f4a6e170 100644 --- a/test/testParametric.jl +++ b/test/testParametric.jl @@ -9,6 +9,8 @@ using Test # - Pose2Pose2 # - Pose2Point2BearingRange # - Pose2Point2 +# - DynPose2VelocityPrior +# - VelPose2VelPose2 @testset "Test PriorPose2 and Pose2Pose2" begin @@ -160,4 +162,31 @@ vardict, result, varIds, Σ = IIF.solveFactorGraphParametric(fg, useCalcFactor=t # IIF.updateParametricSolution(fg, vardict) # pl = plotSLAM2D(fg; lbls=true, solveKey=:parametric, point_size=4pt, drawPoints=false, drawContour=false) -end \ No newline at end of file +end + + +@testset "Test Parametric DynPose2VelocityPrior and VelPose2VelPose2" begin + +fg = LightDFG( solverParams=SolverParams(algorithms=[:default, :parametric])) + +# add first pose locations +addVariable!(fg, :x0, DynPose2; nanosecondtime=0) + +# Prior factor as boundary condition +pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), [0.01; 0.01; 0.001]), MvNormal([10.0;0], [0.1; 0.1])) +addFactor!(fg, [:x0;], pp0) + +addVariable!(fg, :x1, DynPose2; nanosecondtime=1000_000_000) + +# conditional likelihood between Dynamic Point2 +dp2dp2 = VelPose2VelPose2(MvNormal([10.0;0;0], [0.01;0.01;0.001]), MvNormal([0.0;0], [0.1; 0.1])) +addFactor!(fg, [:x0;:x1], dp2dp2) + +ensureAllInitialized!(fg) + +vardict, result, varIds, Σ = IIF.solveFactorGraphParametric!(fg) + +@test isapprox(vardict[:x0].val, [0, 0, 0, 10, 0], atol = 1e-3) +@test isapprox(vardict[:x1].val, [10, 0, 0, 10, 0], atol = 1e-3) + +end diff --git a/test/testhigherdimroots.jl b/test/testhigherdimroots.jl index 7499df2b..4215f48e 100644 --- a/test/testhigherdimroots.jl +++ b/test/testhigherdimroots.jl @@ -14,11 +14,13 @@ end getSample(cfo::CalcFactor{<:RotationTest}, N::Int=1) = (reshape(rand(cfo.factor.z,N),3,:),) # 3 dimensional line, z = [a b][x y]' + c -function (cfo::CalcFactor{<:RotationTest})( res::AbstractVector{<:Real}, - meas, +function (cfo::CalcFactor{<:RotationTest})( meas, var1, var2) # + #FIXME JT - I'm createing new res for simplicity, it may not hold up well though + res = Vector{eltype(var1)}(undef, 3) + z = meas dq = convert(Quaternion, Euler(z...)) @show var1 @@ -30,7 +32,7 @@ function (cfo::CalcFactor{<:RotationTest})( res::AbstractVector{<:Real}, qq = dq*q_conj(q12) @show res vee!(res, convert(TU.so3, qq)) - nothing + return res end rr = RotationTest(MvNormal(zeros(3), 0.001*diagm(ones(3))))