def km_state_update_for_search(FMotion=None,
FMotion_wh=None,
det_pos=None,
param=None,
*args,
**kwargs):
size_state = det_pos
XX = FMotion.X[:, -1]
PP = FMotion.P[:, :, -1]
XX, PP = km_estimation(XX, det_pos[0:2], param, PP)
#np.c_[FMotion.X,XX]
FMotion.X = XX
#np.concatenate((FMotion.P,PP),axis = 2)
FMotion.P = PP
if param.wh_predict == True:
#update wh kalman filter
XX = FMotion_wh.X[:, -1]
PP = FMotion_wh.P[:, :, -1]
XX, PP = km_estimation(XX, det_pos[2:], param, PP)
#np.c_[FMotion.X,XX]
FMotion_wh.X = XX
#np.concatenate((FMotion.P,PP),axis = 2)
FMotion_wh.P = PP
return FMotion, FMotion_wh
def km_state_update(Trk=None,
ymeas=None,
param=None,
fr=None,
*args,
**kwargs):
size_state = estimation_size(Trk, ymeas, fr)
XX = Trk.FMotion.X[:, -1]
PP = Trk.FMotion.P[:, :, -1]
if len(ymeas) != 0:
XX, PP = km_estimation(XX, ymeas[0:2], param, PP)
else:
XX, PP = km_estimation(XX, [], param, PP)
pos_state = np.r_[XX[0], XX[2]]
current_state = np.r_[pos_state[:, np.newaxis],
size_state[:, np.newaxis]].squeeze()
Trk.state.append(current_state)
Trk.FMotion.X = np.c_[Trk.FMotion.X, XX]
Trk.FMotion.P = np.concatenate((Trk.FMotion.P, PP), axis=2)
return Trk
def km_state_predict(FMotion=None,
FMotion_wh=None,
cur_det=None,
param=None,
*args,
**kwargs):
XX = FMotion.X[:, -1]
PP = FMotion.P[:, :, -1]
XX, PP = km_estimation(XX, [], param, PP)
pos_state = np.r_[XX[0], XX[2]]
#predict wh
if param.wh_predict == True:
XX = FMotion_wh.X[:, -1]
PP = FMotion_wh.P[:, :, -1]
XX, PP = km_estimation(XX, [], param, PP)
size_state = np.r_[XX[0], XX[2]]
else:
size_state = np.r_[cur_det[2], cur_det[3]]
predict_state = np.r_[pos_state[:, np.newaxis],
size_state[:, np.newaxis]].squeeze()
return predict_state
def mot_motion_similarity(Refer=None,
Test=None,
param=None,
type_=None,
*args,
**kwargs):
pos_var = param.pos_var
if 'Obs' == type_:
XX = Refer.FMotion.X[:, -1]
refer_pos = [XX[0], XX[2]]
test_pos = Test.pos
mot_sim = motion_affinity(refer_pos, test_pos, pos_var)
else:
if 'Trk' == type_:
fgap = (Test.init_time - Refer.end_time)
if fgap > 0:
init_time = Test.init_time
end_time = Refer.end_time
FX = Refer.FMotion.X[:, init_time]
BX = Test.BMotion.X[:, -1]
BP = Test.BMotion.P[:, :, -1]
while fgap > 0:
BP = BP.squeeze()
BX, BP = km_estimation(BX, [], param, BP)
fgap = fgap - 1
# Forward motion
refer_pos = [FX[0], FX[2]]
test_pos = [Test.BMotion.X[0, -1], Test.BMotion.X[2, -1]]
mot_sim1 = motion_affinity(refer_pos, test_pos, pos_var)
#Backward motion
refer_pos = [BX[0], BX[2]]
test_pos = [
Refer.FMotion.X[0, end_time], Refer.FMotion.X[2, end_time]
]
mot_sim2 = motion_affinity(refer_pos, test_pos, pos_var)
mot_sim = mot_sim1 * mot_sim2
else:
mot_sim = 0
else:
warning('Unexpected type. Choose Obs or Trk.')
return mot_sim