Skip to content

Commit 2defb91

Browse files
dehanndehann
authored
Merge pull request #409 from JuliaRobotics/21Q1/maint/calcfactorfluxtest
new flux factor test and update to CalcFactor
2 parents faae3cb + 58f2c45 commit 2defb91

File tree

5 files changed

+172
-107
lines changed

5 files changed

+172
-107
lines changed

‎src/factors/flux/FluxModelsPose2Pose2.jl renamed to ‎attic/FluxModelsPose2Pose2.jl

Lines changed: 71 additions & 56 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ buildPose2OdoNN_01_FromElements();
1313
# FluxModelsPose2Pose2
1414

1515
# the factor definitions
16-
export FluxModelsPose2Pose2, sampleFluxModelsPose2Pose2, PackedFluxModelsPose2Pose2
16+
export FluxModelsPose2Pose2, PackedFluxModelsPose2Pose2
1717
# some utilities
1818
export setShuffleAll!, setNaiveFracAll!
1919

@@ -33,7 +33,6 @@ struct FluxModelsPose2Pose2{P,D<:AbstractArray,M<:SamplableBelief} <: AbstractRe
3333
naiveFrac::Ref{Float64}
3434
# TODO type stability likely wants parameter Pose2Pose2{<:SamplableBelief}
3535
Zij::Pose2Pose2{<:SamplableBelief}
36-
specialSampler::Function # special keyword field name used to invoke 'specialSampler' logic
3736
DT::Ref{Float64}
3837
shuffle::Ref{Bool}
3938
end
@@ -42,14 +41,13 @@ end
4241

4342
function calcVelocityInterPose2!( nfb::FluxModelsPose2Pose2,
4443
iPts::AbstractMatrix{<:Real},
45-
jPts::AbstractMatrix{<:Real},
46-
idx::Int )
44+
jPts::AbstractMatrix{<:Real})
4745
#
4846
# DXY[1:2,i] .= TransformUtils.R(iPts[3,i])'*DXY[1:2,i]
49-
nfb.joyVelData[1,3:4] .= jPts[1:2,idx]
50-
nfb.joyVelData[1,3:4] .-= iPts[1:2,idx]
47+
nfb.joyVelData[1,3:4] .= jPts[1:2]
48+
nfb.joyVelData[1,3:4] .-= iPts[1:2]
5149
nfb.joyVelData[1,3:4] ./= nfb.DT[] # convert to velocity
52-
nfb.joyVelData[1,3:4] .= TransformUtils.R(iPts[3,idx])'*nfb.joyVelData[1,3:4]
50+
nfb.joyVelData[1,3:4] .= TransformUtils.R(iPts[3])'*nfb.joyVelData[1,3:4]
5351
# just set zero if something is wrong
5452
if isnan(nfb.joyVelData[1,3]) || isinf(abs(nfb.joyVelData[1,3])) || isnan(nfb.joyVelData[1,4]) || isinf(abs(nfb.joyVelData[1,4]))
5553
nfb.joyVelData[1,3:4] .= 0.0
@@ -60,32 +58,32 @@ function calcVelocityInterPose2!( nfb::FluxModelsPose2Pose2,
6058
end
6159

6260

63-
function calcVelocityInterPose2!( nfb::FluxModelsPose2Pose2,
64-
iPts::AbstractMatrix{<:Real},
65-
jPts::AbstractMatrix{<:Real} )
66-
#
67-
@assert size(jPts,2) == size(iPts,2) "sampleFluxModelsPose2Pose2 can currently only evaluate equal population size variables"
68-
69-
# calculate an average velocity component
70-
if nfb.DT[] == 0
71-
# DXY = (@view jPts[1:2,:]) - (@view jPts[1:2,:])
72-
# rotate delta position from world to local iX frame
73-
for i in 1:size(iPts,2)
74-
calcVelocityInterPose2!(nfb, iPts, jPts, i)
75-
end
76-
else
77-
nfb.joyVelData[:,3:4] .= 0.0
78-
end
79-
nothing
80-
end
81-
61+
# function calcVelocityInterPose2!( nfb::FluxModelsPose2Pose2,
62+
# iPts::AbstractMatrix{<:Real},
63+
# jPts::AbstractMatrix{<:Real} )
64+
# #
65+
# @assert size(jPts,2) == size(iPts,2) "sampleFluxModelsPose2Pose2 can currently only evaluate equal population size variables"
66+
67+
# # calculate an average velocity component
68+
# if nfb.DT[] == 0
69+
# # DXY = (@view jPts[1:2,:]) - (@view jPts[1:2,:])
70+
# # rotate delta position from world to local iX frame
71+
# for i in 1:size(iPts,2)
72+
# calcVelocityInterPose2!(nfb, iPts, jPts, i)
73+
# end
74+
# else
75+
# nfb.joyVelData[:,3:4] .= 0.0
76+
# end
77+
# nothing
78+
# end
8279

83-
function sampleFluxModelsPose2Pose2(nfb::FluxModelsPose2Pose2,
84-
N::Int,
85-
fmd::FactorMetadata,
86-
Xi::DFGVariable,
87-
Xj::DFGVariable )
80+
# sampleFluxModelsPose2Pose2
81+
function IIF.getSample(cfo::CalcFactor{<:FluxModelsPose2Pose2}, N::Int=1)
8882
#
83+
nfb = cfo.factor
84+
fmd = cfo.metadata
85+
Xi = fmd.fullvariables[1] # X
86+
Xj = fmd.fullvariables[2] # A / B
8987

9088
# get the naive samples
9189
# model samples (all for theta at this time)
@@ -97,14 +95,14 @@ function sampleFluxModelsPose2Pose2(nfb::FluxModelsPose2Pose2,
9795
# number of predictors to choose from, and choose random subset
9896
numModels = length(nfb.allPredModels)
9997
allPreds = 1:numModels |> collect # 1:Npreds |> collect
100-
# TODO -- compensate when there arent enough prediction models
98+
# TODO -- compensate when there aren't enough prediction models
10199
if numModels < N
102100
repeat(allPreds, ceil(Int, (N-numModels)/N) + 1)
103101
allPreds = allPreds[1:N]
104102
end
105103
# samples for the order in which to use models, dont shuffle if N models
106104
# can suppress shuffle for NN training purposes
107-
1 < numModels && nfb.shuffle[] ? shuffle!(allPreds) : nothing
105+
1 < numModels && nfb.shuffle[] ? Random.shuffle!(allPreds) : nothing
108106

109107
# cache the time difference estimate
110108
nfb.DT[] = (getTimestamp(Xj) - getTimestamp(Xi)).value * 1e-3
@@ -113,43 +111,60 @@ function sampleFluxModelsPose2Pose2(nfb::FluxModelsPose2Pose2,
113111
end
114112

115113
# Convenience function to help call the right constuctor
116-
FluxModelsPose2Pose2( allModels::Vector{P},
117-
jvd::D,
118-
naiveModel::M,
119-
naiveFrac::Real=0.5,
120-
ss::Function=sampleFluxModelsPose2Pose2,
121-
DT::Real=0.0,
122-
shuffle::Bool=true ) where {P, M <: SamplableBelief, D <: AbstractMatrix} = FluxModelsPose2Pose2{P,D,M}(
114+
FluxModelsPose2Pose2( allModels::Vector,
115+
jvd::AbstractMatrix,
116+
naiveModel::SamplableBelief,
117+
naiveFrac::Float64=0.5,
118+
DT::Float64=0.0,
119+
shuffle::Bool=true ) = FluxModelsPose2Pose2(
123120
allModels,
124121
jvd,
125122
naiveModel,
126-
naiveFrac,
123+
Ref(naiveFrac),
127124
Pose2Pose2(MvNormal(zeros(3),diagm(ones(3)))), # this dummy distribution does not get used
128-
ss,
129-
DT,
130-
shuffle )
125+
Ref(DT),
126+
Ref(shuffle) )
131127
#
128+
# struct FluxModelsPose2Pose2{P,D<:AbstractArray,M<:SamplableBelief} <: AbstractRelativeRoots
129+
# allPredModels::Vector{P}
130+
# joyVelData::D
131+
# naiveModel::M
132+
# naiveFrac::Ref{Float64}
133+
# # TODO type stability likely wants parameter Pose2Pose2{<:SamplableBelief}
134+
# Zij::Pose2Pose2{<:SamplableBelief}
135+
# DT::Ref{Float64}
136+
# shuffle::Ref{Bool}
137+
# end
132138

133139

134-
function (nfb::FluxModelsPose2Pose2)(
135-
res::AbstractArray{<:Real},
136-
userdata::FactorMetadata,
137-
idx::Int,
138-
meas::Tuple{AbstractArray{<:Real},AbstractArray{<:Int},AbstractArray{<:Int}},
139-
Xi::AbstractArray{<:Real,2},
140-
Xj::AbstractArray{<:Real,2} )
140+
function (cfo::CalcFactor{<:FluxModelsPose2Pose2})(meas1,meas2,meas3,Xi,Xj)
141+
# function (nfb::FluxModelsPose2Pose2)(
142+
# res::AbstractArray{<:Real},
143+
# userdata::FactorMetadata,
144+
# idx::Int,
145+
# meas::Tuple{AbstractArray{<:Real},AbstractArray{<:Int},AbstractArray{<:Int}},
146+
# Xi::AbstractArray{<:Real,2},
147+
# Xj::AbstractArray{<:Real,2} )
141148
#
149+
userdata = cfo.metadata
150+
nfb = cfo.factor
151+
fmd = cfo.metadata
152+
meas = (meas1, meas2, meas3)
153+
154+
## MAYBE JUST MOVE THIS INTO getSample and be done
142155
# if, use prediction sample
143-
if meas[2][idx] == 2
156+
if meas[2] == 2 # meas2 == 2
144157
# get live velocity estimate for each sample (nfb.joyVelData[:,3:4])
145-
calcVelocityInterPose2!(nfb, Xi, Xj, idx)
158+
calcVelocityInterPose2!(nfb, Xi, Xj)
146159
# predict odom for this sample from a specific prediction model
147-
meas[1][1:2,idx] = nfb.allPredModels[meas[3][idx]](nfb.joyVelData)
160+
meas[1][1:2] = nfb.allPredModels[meas[3]](nfb.joyVelData)
148161
end
149162

150163
# calculate the error for that measurement sample as Pose2Pose2
151-
nfb.Zij(res, userdata, idx, (meas[1],), Xi, Xj)
152-
nothing
164+
#TODO
165+
cfZij = CalcFactor( nfb.Zij, nothing, 0, 0, (), [])
166+
res = cfZij((meas[1],), Xi, Xj)
167+
153168
end
154169

155170
"""

‎src/RoME.jl

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -297,7 +297,6 @@ function __init__()
297297
@info "RoME is adding Flux related functionality."
298298
include("factors/flux/models/Pose2OdoNN_01.jl") # until a better way is found to deserialize
299299
include("factors/flux/MixtureFluxPose2Pose2.jl")
300-
# include("factors/flux/FluxModelsPose2Pose2.jl")
301300
end
302301
end
303302

‎src/factors/flux/MixtureFluxPose2Pose2.jl

Lines changed: 50 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@ struct MixtureFluxPose2Pose2{F <: FunctorInferenceType} <: AbstractRelativeRoots
1212
Zij::F
1313
# delta time between variables
1414
DT::Ref{Float64}
15-
specialSampler::Function
1615
end
1716

1817
mutable struct PackedMixtureFluxPose2Pose2 <: PackedInferenceType
@@ -21,18 +20,24 @@ mutable struct PackedMixtureFluxPose2Pose2 <: PackedInferenceType
2120
specialSampler::String
2221
end
2322

24-
function sampleMixtureFluxPose2Pose2( nfb::MixtureFluxPose2Pose2,
25-
N::Int,
26-
fmd::FactorMetadata,
27-
legacy... )
23+
24+
function IIF.getSample(cfo::CalcFactor{<:MixtureFluxPose2Pose2}, N::Int=1)
2825
#
26+
nfb = cfo.factor
27+
fmd = cfo.metadata
28+
2929
# only calculate once after default value of 0
3030
if nfb.DT[] == 0
3131
# cache the time difference estimate
3232
nfb.DT[] = (getTimestamp(fmd.fullvariables[2]) - getTimestamp(fmd.fullvariables[1])).value * 1e-3
3333
end
3434

35-
return getSample(nfb.Zij, N)
35+
cf_ = CalcFactor( cfo.factor.Zij, cfo.metadata, 0, length(cfo._legacyMeas), cfo._legacyMeas, cfo._legacyParams)
36+
37+
smpl = getSample(cf_, N)[1]
38+
39+
return (smpl, cf_.factor.labels)
40+
3641
end
3742

3843

@@ -44,8 +49,7 @@ function MixtureFluxPose2Pose2( fluxmodels::AbstractVector,
4449
diversity=[0.5;0.5];
4550
DT::Real=0,
4651
shuffle::Bool=true,
47-
serializeHollow::Bool=false,
48-
specialSampler::Function=sampleMixtureFluxPose2Pose2 )
52+
serializeHollow::Bool=false )
4953
#
5054
mix = MixtureFluxModels(Pose2Pose2,
5155
fluxmodels,
@@ -57,7 +61,7 @@ function MixtureFluxPose2Pose2( fluxmodels::AbstractVector,
5761
shuffle=shuffle,
5862
serializeHollow=serializeHollow)
5963
#
60-
MixtureFluxPose2Pose2(mix, Base.RefValue{Float64}(DT), specialSampler)
64+
MixtureFluxPose2Pose2(mix, Base.RefValue{Float64}(DT))
6165
end
6266

6367

@@ -85,67 +89,67 @@ end
8589

8690

8791
function calcVelocityInterPose2!( nfb::MixtureFluxPose2Pose2,
88-
iPts::AbstractMatrix{<:Real},
89-
jPts::AbstractMatrix{<:Real},
90-
idx::Int )
92+
iPts::AbstractVector{<:Real},
93+
jPts::AbstractVector{<:Real} )
9194
#
9295
joyVelData = nfb.Zij.components.fluxnn.data
9396
# DXY[1:2,i] .= TransformUtils.R(iPts[3,i])'*DXY[1:2,i]
94-
joyVelData[1,3:4] .= jPts[1:2,idx]
95-
joyVelData[1,3:4] .-= iPts[1:2,idx]
97+
joyVelData[1,3:4] .= jPts[1:2]
98+
joyVelData[1,3:4] .-= iPts[1:2]
9699
joyVelData[1,3:4] ./= nfb.DT[] # convert to velocity
97-
joyVelData[1,3:4] .= TransformUtils.R(iPts[3,idx])'*joyVelData[1,3:4]
100+
joyVelData[1,3:4] .= TransformUtils.R(iPts[3])'*joyVelData[1,3:4]
98101
# just set zero if something is wrong
99102
if isnan(joyVelData[1,3]) || isinf(abs(joyVelData[1,3])) || isnan(joyVelData[1,4]) || isinf(abs(joyVelData[1,4]))
100103
joyVelData[1,3:4] .= 0.0
101104
end
102105
for i in 2:size(joyVelData,1)
103106
joyVelData[i,3:4] .= joyVelData[1,3:4]
104107
end
108+
return nothing
105109
end
106110

107111

108-
function calcVelocityInterPose2!( nfb::MixtureFluxPose2Pose2,
109-
iPts::AbstractMatrix{<:Real},
110-
jPts::AbstractMatrix{<:Real} )
111-
#
112-
@assert size(jPts,2) == size(iPts,2) "calcVelocityInterPose2! can currently only evaluate equal population size variables"
113-
114-
# calculate an average velocity component
115-
if nfb.DT[] == 0
116-
# DXY = (@view jPts[1:2,:]) - (@view jPts[1:2,:])
117-
# rotate delta position from world to local iX frame
118-
for i in 1:size(iPts,2)
119-
calcVelocityInterPose2!(nfb, iPts, jPts, i)
120-
end
121-
else
122-
nfb.joyVelData[:,3:4] .= 0.0
123-
end
124-
nothing
125-
end
126-
112+
# function calcVelocityInterPose2!( nfb::MixtureFluxPose2Pose2,
113+
# iPts::AbstractMatrix{<:Real},
114+
# jPts::AbstractMatrix{<:Real} )
115+
# #
116+
# @assert size(jPts,2) == size(iPts,2) "calcVelocityInterPose2! can currently only evaluate equal population size variables"
117+
118+
# # calculate an average velocity component
119+
# if nfb.DT[] == 0
120+
# # DXY = (@view jPts[1:2,:]) - (@view jPts[1:2,:])
121+
# # rotate delta position from world to local iX frame
122+
# for i in 1:size(iPts,2)
123+
# calcVelocityInterPose2!(nfb, iPts, jPts, i)
124+
# end
125+
# else
126+
# nfb.joyVelData[:,3:4] .= 0.0
127+
# end
128+
# nothing
129+
# end
127130

128131

129-
function (nfb::MixtureFluxPose2Pose2)(
130-
res::AbstractArray{<:Real},
131-
fmd::FactorMetadata,
132-
idx::Int,
133-
meas::Tuple,
134-
Xi::AbstractArray{<:Real,2},
135-
Xj::AbstractArray{<:Real,2} )
132+
function (cfo::CalcFactor{<:MixtureFluxPose2Pose2})(meas1, meas2, Xi, Xj)
136133
#
134+
userdata = cfo.metadata
135+
nfb = cfo.factor
136+
fmd = cfo.metadata
137+
137138
# if, use prediction sample
138-
if meas[2][idx] == 2
139+
if meas2 == 2
139140
# get live velocity estimate for each sample (nfb.joyVelData[:,3:4])
140-
calcVelocityInterPose2!(nfb, Xi, Xj, idx)
141+
calcVelocityInterPose2!(nfb, Xi, Xj)
141142
# predict odom for this sample from a specific prediction model
142143
# NOTE, already integrated as part of Mixture
143144
# meas[1][1:2,idx] = nfb.allPredModels[meas[3][idx]](nfb.joyVelData)
144145
end
145146

146147
# calculate the error for that measurement sample as Pose2Pose2
147-
nfb.Zij(res, fmd, idx, (meas[1],), Xi, Xj)
148-
nothing
148+
#TODO
149+
cfZij = CalcFactor( cfo.factor.Zij.mechanics, cfo.metadata, 0, length(cfo._legacyMeas), cfo._legacyMeas, cfo._legacyParams)
150+
151+
return cfZij(meas1, Xi, Xj)
152+
149153
end
150154

151155

@@ -165,7 +169,6 @@ FluxModelsPose2Pose2( allModels::Vector{P},
165169
jvd::D,
166170
naiveModel::M,
167171
naiveFrac::Real=0.5,
168-
specialSampler::Function=sampleMixtureFluxPose2Pose2,
169172
DT::Real=0.0,
170173
shuffle::Bool=true ) where {P, M <: SamplableBelief, D <: AbstractMatrix} = MixtureFluxPose2Pose2(
171174
allModels,
@@ -175,8 +178,7 @@ FluxModelsPose2Pose2( allModels::Vector{P},
175178
[naiveModel;],
176179
[1-naiveFrac;naiveFrac],
177180
DT=DT,
178-
shuffle=shuffle,
179-
specialSampler=specialSampler )
181+
shuffle=shuffle )
180182
#
181183

182184

0 commit comments

Comments
 (0)