def segmentSequenceKeyPose(_in, ws, thres):
variation = np.full([1, _in.shape[1]], np.nan)
for t in range(math.ceil(ws / 2), _in.shape[1] - math.floor(ws / 2) + 1):
sigma = 0
w = math.floor(ws / 2)
for d in range(int(_in.shape[0] / 4)):
muMan = _in[d * 4:d * 4 + 4, t - 1:t]
sigma = sigma + np.linalg.norm(
logmap(_in[d * 4:d * 4 + 4, t - w - 1:t + w], muMan))**2
sigma = sigma / ws
variation[0][t - 1] = sigma
mintab = np.array([])
kp = 1
for t in range(variation.shape[1]):
if kp == 1:
if variation[0][t] > thres:
mintab = np.hstack((mintab, t + 1))
kp = 0
else:
if variation[0][t] < thres:
mintab = np.hstack((mintab, t + 1))
kp = 1
cuts = mintab.astype(
int) # in Python, indice begins with 0 while in Matla b with 1
return cuts, variation
def removeStart(_in, ws, thres, dec):
for t in range(math.ceil(ws / 2), _in.shape[1] - math.floor(ws / 2) + 1):
sigma = 0
w = math.floor(ws / 2)
for d in range(int(_in.shape[0] / 4)):
muMan = _in[d * 4:d * 4 + 4, t - 1:t]
sigma = sigma + np.linalg.norm(
logmap(_in[d * 4:d * 4 + 4, t - w - 1:t + w], muMan))**2
sigma = sigma / ws
if sigma > thres:
deb = max(1, t - dec)
out = _in[:, deb - 1:]
return out, deb
deb = 1
out = _in
return out, deb
def segmentSequence(_in, ws, thres): ##return 1-d vecteur and matrix
variation = np.full((1, _in.shape[1]), np.nan)
for t in range(math.ceil(ws / 2), _in.shape[1] - math.floor(ws / 2) + 1):
sigma = 0
w = math.floor(ws / 2)
for d in range(int(_in.shape[0] / 4)):
muMan = _in[d * 4: d * 4 + 4, t - 1:t]
sigma = sigma + np.linalg.norm(logmap(_in[d * 4:d * 4 + 4, t - w - 1:t + w], muMan)) ** 2
sigma = sigma / ws
variation[0][t - 1] = sigma
mintab = peakdet(variation, thres)[1]
if mintab.shape[1] > 0:
cuts = mintab[:, 0] # in Python, indice begins with 0 while in Matalb with 1
else:
cuts = np.array([])
return cuts.astype(int), variation
def temporalAlignment(trainD, testD, fast, sizeData):
# Dynamic programming to align two sequences train and test
# fast may be set to 1 to speed process by using small windows of 70
# Compute distances
DM = np.zeros((sizeData, sizeData))
for row in range(0, sizeData): ##TODO : possible optimisation
DMline = logmap(testD['lElbow ori'], trainD['lElbow ori'][:,
row:row + 1])
DMline = np.vstack((DMline,
logmap(testD['lWrist ori'],
trainD['lWrist ori'][:, row:row + 1])))
DMline = np.vstack((DMline,
logmap(testD['lShoulder ori'],
trainD['lShoulder ori'][:, row:row + 1])))
DMline = np.vstack((DMline,
logmap(testD['rElbow ori'],
trainD['rElbow ori'][:, row:row + 1])))
DMline = np.vstack((DMline,
logmap(testD['rWrist ori'],
trainD['rWrist ori'][:, row:row + 1])))
DMline = np.vstack((DMline,
logmap(testD['rShoulder ori'],
trainD['rShoulder ori'][:, row:row + 1])))
DM[row, :] = np.linalg.norm(DMline, axis=0)
if fast == 1:
for row in range(sizeData - 70):
for col in range(70 + row, sizeData):
DM[row][col] = 5
for col in range(sizeData - 70):
for row in range(70 + col, sizeData):
DM[row][col] = 5
p, q = dp(DM) ## execution time : 1s
# alignment
r = np.zeros(sizeData)
for t in range(sizeData):
ind = np.where(p == t)[0]
if ind.size == 0:
r[t] = p[t]
else:
r[t] = q[ind[0]]
# out
r = r.astype(int)
out = {}
for key in testD:
out[key] = testD[key][:, r]
return out, r
def temporalAlignmentEval(m_cuts, trainD, testD, fast, sizeData):
keypose = 0
prevDist = 5
poseTracking = np.zeros(m_cuts.size)
motionStarted = 0
performedEntirely = 0
if fast == 1:
resampleSize = 10
else:
resampleSize = 1
row = np.arange(0, sizeData + 1, resampleSize).astype(int)
col = np.arange(0, sizeData + 1, resampleSize).astype(int)
DM = np.zeros((row.size, col.size))
if row[-1] == sizeData:
row[-1] = sizeData - 1
if col[-1] == sizeData:
col[-1] = sizeData - 1
for irow in row: ##TODO : possible optimisation
Line = logmap(testD['lElbow ori'][:, col],
trainD['lElbow ori'][:, irow:irow + 1])
Line = np.vstack((Line,
logmap(testD['lWrist ori'][:, col],
trainD['lWrist ori'][:, irow:irow + 1])))
Line = np.vstack((Line,
logmap(testD['lShoulder ori'][:, col],
trainD['lShoulder ori'][:, irow:irow + 1])))
Line = np.vstack((Line,
logmap(testD['rElbow ori'][:, col],
trainD['rElbow ori'][:, irow:irow + 1])))
Line = np.vstack((Line,
logmap(testD['rWrist ori'][:, col],
trainD['rWrist ori'][:, irow:irow + 1])))
Line = np.vstack((Line,
logmap(testD['rShoulder ori'][:, col],
trainD['rShoulder ori'][:, irow:irow + 1])))
DM[np.where(row == irow), :] = np.linalg.norm(Line, axis=0)
if fast == 1:
for i in range(15):
for j in range(15 + i, 31):
DM[i][j] = 0
for j in range(15):
for i in range(15 + j, 31):
DM[i][j] = 0
## keyposeTracking
i = m_cuts[keypose] - 1
Line = logmap(testD['lElbow ori'][:, col], trainD['lElbow ori'][:,
i:i + 1])
Line = np.vstack((Line,
logmap(testD['lWrist ori'][:, col],
trainD['lWrist ori'][:, i:i + 1])))
Line = np.vstack((Line,
logmap(testD['lShoulder ori'][:, col],
trainD['lShoulder ori'][:, i:i + 1])))
Line = np.vstack((Line,
logmap(testD['rElbow ori'][:, col],
trainD['rElbow ori'][:, i:i + 1])))
Line = np.vstack((Line,
logmap(testD['rWrist ori'][:, col],
trainD['rWrist ori'][:, i:i + 1])))
Line = np.vstack((Line,
logmap(testD['rShoulder ori'][:, col],
trainD['rShoulder ori'][:, i:i + 1])))
dist = np.linalg.norm(Line, axis=0)
for n in range(col.size):
if performedEntirely == 0:
if prevDist < 1.7 and dist[n] > prevDist:
poseTracking[keypose] = 1
keypose += 1
if keypose == m_cuts.size:
performedEntirely = 1
else:
i = m_cuts[keypose] - 1
Line = logmap(testD['lElbow ori'][:, col],
trainD['lElbow ori'][:, i:i + 1])
Line = np.vstack((Line,
logmap(testD['lWrist ori'][:, col],
trainD['lWrist ori'][:,
i:i + 1])))
Line = np.vstack(
(Line,
logmap(testD['lShoulder ori'][:, col],
trainD['lShoulder ori'][:, i:i + 1])))
Line = np.vstack((Line,
logmap(testD['rElbow ori'][:, col],
trainD['rElbow ori'][:,
i:i + 1])))
Line = np.vstack((Line,
logmap(testD['rWrist ori'][:, col],
trainD['rWrist ori'][:,
i:i + 1])))
Line = np.vstack(
(Line,
logmap(testD['rShoulder ori'][:, col],
trainD['rShoulder ori'][:, i:i + 1])))
dist = np.linalg.norm(Line, axis=0)
prevDist = dist[n]
# in Matlab, col==size(testD{1}.data,2 and poseTracking(keypose)==0 useless
if keypose == m_cuts.size - 1 and prevDist < 1.4:
poseTracking[keypose] = 1
performedEntirely = 1
# distance from init
Line = logmap(testD['lElbow ori'][:, col], testD['lElbow ori'][:, 0:1])
Line = np.vstack(
(Line, logmap(testD['lWrist ori'][:, col], testD['lWrist ori'][:,
0:1])))
Line = np.vstack((Line,
logmap(testD['lShoulder ori'][:, col],
testD['lShoulder ori'][:, 0:1])))
Line = np.vstack(
(Line, logmap(testD['rElbow ori'][:, col], testD['rElbow ori'][:,
0:1])))
Line = np.vstack(
(Line, logmap(testD['rWrist ori'][:, col], testD['rWrist ori'][:,
0:1])))
Line = np.vstack((Line,
logmap(testD['rShoulder ori'][:, col],
testD['rShoulder ori'][:, 0:1])))
Line = np.linalg.norm(Line, axis=0)
distFromInit = np.where(Line < 1.4, 0, 1)
# poseTracking
if np.sum(distFromInit) / distFromInit.size > 0.2:
motionStarted = 1
# dynamic programming
p, q = dp(DM) ## execution time : 1s
# alignment
r = np.zeros(DM.shape[1])
for t in range(DM.shape[1]):
ind = np.where(p == t)[0]
if ind.size == 0:
r[t] = p[t]
else:
r[t] = q[ind[0]]
if performedEntirely == 1 and r[-1] < np.max(p):
r[-2] = r[-3]
r[-1] = r[-3]
r = r.astype(int)
r = row[r] #"row" equals "kf" in Matlab
rfull = np.zeros(sizeData)
rfull[row] = r
if fast == 1:
for i in range(r.size - 1):
if r[i] == r[i + 1]:
temp = np.ones(resampleSize - 1) * r[i]
else:
temp = round(
np.linspace(r[i] + 1, r[i + 1] - 1, resampleSize - 1))
rfull[range(i * resampleSize + 1, (i + 1) * resampleSize)] = temp
rfull = rfull.astype(int)
r = rfull
# no need to check r=0, this case is impossible
out = {}
for key in testD:
out[key] = testD[key][:, r]
return out, r, performedEntirely, poseTracking, motionStarted, distFromInit
def computeLikelihoods(m_dt, m_nbStates, m_nbVar, m_Priors, m_Mu, m_Sigma,
m_MuMan, dataTest, sizeData):
xIn = np.arange(1, sizeData + 1) * m_dt
L = np.zeros((m_nbStates, sizeData))
Data_gaussPDF = np.zeros((m_nbVar, sizeData, m_nbStates))
Lglobal = {}
Lbodypart = {}
Ljoints = {}
# Global(orientation + position)
for i in range(m_nbStates):
Data_gaussPDF[0, :, i] = xIn - m_MuMan[0, i]
Data_gaussPDF[1:4, :, i] = logmap(dataTest['lElbow ori'], m_MuMan[1:5,
i])
Data_gaussPDF[4:7, :, i] = logmap(dataTest['lWrist ori'], m_MuMan[5:9,
i])
Data_gaussPDF[7:10, :, i] = logmap(dataTest['lShoulder ori'],
m_MuMan[9:13, i])
Data_gaussPDF[10:13, :, i] = logmap(dataTest['rElbow ori'],
m_MuMan[13:17, i])
Data_gaussPDF[13:16, :, i] = logmap(dataTest['rWrist ori'],
m_MuMan[17:21, i])
Data_gaussPDF[16:19, :, i] = logmap(dataTest['rShoulder ori'],
m_MuMan[21:25, i])
Data_gaussPDF[19:22, :, i] = logmap(dataTest['mSpine ori'],
m_MuMan[25:29, i])
Data_gaussPDF[22:25, :, i] = logmap(dataTest['mShoulder ori'],
m_MuMan[29:33, i])
Data_gaussPDF[25:28, :, i] = logmap(dataTest['Neck ori'],
m_MuMan[33:37, i])
Data_gaussPDF[28:31, :,
i] = dataTest['lElbow rel_pos'] - m_MuMan[37:40, i:i + 1]
Data_gaussPDF[31:34, :,
i] = dataTest['lWrist rel_pos'] - m_MuMan[40:43, i:i + 1]
Data_gaussPDF[34:37, :,
i] = dataTest['lShoulder rel_pos'] - m_MuMan[43:46,
i:i + 1]
Data_gaussPDF[37:40, :,
i] = dataTest['rElbow rel_pos'] - m_MuMan[46:49, i:i + 1]
Data_gaussPDF[40:43, :,
i] = dataTest['rWrist rel_pos'] - m_MuMan[49:52, i:i + 1]
Data_gaussPDF[43:46, :,
i] = dataTest['rShoulder rel_pos'] - m_MuMan[52:55,
i:i + 1]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[:, :, i], m_Mu[:, i],
m_Sigma[:, :, i])
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
Lglobal['Global'] = Lnode(LL, score)
# Orientations
# out = 2:28 omitted to have a faster calulation
sigma = np.zeros((28, 28))
mu = np.zeros(28)
for i in range(m_nbStates):
mu[:] = m_Mu[:28, i]
sigma[:, :] = m_Sigma[:28, :28, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[:28, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
Lglobal['Orientations'] = Lnode(LL, score)
# Positions
# out = 29:46 omitted to have a faster calulation
sigma = np.zeros((19, 19))
mu = np.zeros(19)
index = [
0, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44,
45
]
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[0, :] = m_Sigma[0, index, i]
sigma[1:, :] = m_Sigma[28:46, index, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
Lglobal['Positions'] = Lnode(LL, score)
# Body Part
left_arm = {}
right_arm = {}
# Left Arm Global
index = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 28, 29, 30, 31, 32, 33, 34, 35, 36]
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[:10, :] = m_Sigma[:10, index, i]
sigma[10:, :] = m_Sigma[28:37, index, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
left_arm['Left Arm Global'] = Lnode(LL, score)
# Right Arm Global
index = [
0, 10, 11, 12, 13, 14, 15, 16, 17, 18, 37, 38, 39, 40, 41, 42, 43, 44,
45
]
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[0, :] = m_Sigma[0, index, i]
sigma[1:10, :] = m_Sigma[10:19, index, i]
sigma[10:, :] = m_Sigma[37:46, index, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
right_arm['Right Arm Global'] = Lnode(LL, score)
# Left Arm Orientation
sigma = np.zeros((10, 10))
mu = np.zeros(10)
for i in range(m_nbStates):
mu[:] = m_Mu[:10, i]
sigma[:, :] = m_Sigma[:10, :10, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[:10, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
left_arm['Left Arm Orientation'] = Lnode(LL, score)
# Left Arm Postion
index = [0, 28, 29, 30, 31, 32, 33, 34, 35, 36]
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[0, :] = m_Sigma[0, index, i]
sigma[1:, :] = m_Sigma[28:37, index, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
left_arm['Left Arm Position'] = Lnode(LL, score)
# Right Arm Orientation
index = [0, 10, 11, 12, 13, 14, 15, 16, 17, 18]
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[0, :] = m_Sigma[0, index, i]
sigma[1:, :] = m_Sigma[10:19, index, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
right_arm['Right Arm Orientation'] = Lnode(LL, score)
# Right Arm Position
index = [0, 37, 38, 39, 40, 41, 42, 43, 44, 45]
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[0, :] = m_Sigma[0, index, i]
sigma[1:, :] = m_Sigma[37:46, index, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
right_arm['Right Arm Position'] = Lnode(LL, score)
Lbodypart['Left Arm'] = left_arm
Lbodypart['Right Arm'] = right_arm
# Colonne Global (only position)
Colonne = {}
index = [0, 19, 20, 21, 22, 23, 24, 25, 26, 27]
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[0, :] = m_Sigma[0, index, i]
sigma[1:, :] = m_Sigma[19:28, index, i]
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu, sigma)
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
Colonne['Colonne Global'] = Lnode(LL, score)
Lbodypart['Colonne'] = Colonne
# Per Joints
oriL = np.zeros((m_nbStates, sizeData))
posL = np.zeros((m_nbStates, sizeData))
jointNames1 = [
'lElbow', 'lWrist', 'lShoulder', 'rElbow', 'rWrist', 'rShoulder'
]
jointNames2 = ['mSpine', 'mShoulder', 'Neck']
index = np.array([0, 1, 2, 3, 28, 29, 30])
a = [0, 4, 5, 6]
sigma = np.zeros((7, 7))
mu = np.zeros(7)
sigma_ = np.zeros((4, 4))
mu_ = np.zeros(4)
# first six
for name in jointNames1:
tmp = {}
for i in range(m_nbStates):
mu[:] = m_Mu[index, i]
sigma[:, :] = m_Sigma[np.ix_(index, index, [i])].reshape((7, 7))
# Global
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu,
sigma)
# Orientations
mu_[:] = mu[:4]
sigma_[:] = sigma[:4, :4]
oriL[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index[:4], :, i],
mu_, sigma_)
# Positions
index_ = index[a]
mu_[:] = mu[a]
sigma_[:] = sigma[np.ix_(a, a)]
posL[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index_, :, i],
mu_, sigma_)
# Global
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
tmp[name + ' Global'] = Lnode(LL, score)
# Orientations
LL = np.log(np.sum(oriL, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
tmp[name + ' Orientations'] = Lnode(LL, score)
# Positions
LL = np.log(np.sum(posL, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
tmp[name + ' Positions'] = Lnode(LL, score)
Ljoints[name] = tmp.copy()
index += 3
index[0] -= 3
# last three
index = index[:4]
for name in jointNames2:
tmp = {}
for i in range(m_nbStates):
mu_[:] = m_Mu[index, i]
sigma_[:, :] = m_Sigma[np.ix_(index, index, [i])].reshape((4, 4))
L[i, :] = m_Priors[i] * gaussPDF(Data_gaussPDF[index, :, i], mu_,
sigma_)
# Global
LL = np.log(np.sum(L, axis=0))
LL = np.where(LL < -2000, -2000, LL)
score = np.mean(LL)
tmp[name + ' Global'] = Lnode(LL, score)
Ljoints[name] = tmp.copy()
index += 3
index[0] -= 3
return Lglobal, Lbodypart, Ljoints
def learnGMMmodel(model, u, xIn, xOut, nbSamples, nbIterEM, nbIter, nbData):
init_GMM_kbins(u, model, nbSamples)
# MuMan
mu_exp = np.array([[0], [1], [0], [0]])
model.MuMan = np.zeros((model.nbVarMan, model.nbStates))
model.MuMan[0, :] = model.Mu[0, :]
model.MuMan[1:5, :] = expmap(model.Mu[1:4], mu_exp)
model.MuMan[5:9, :] = expmap(model.Mu[4:7], mu_exp)
model.MuMan[9:13, :] = expmap(model.Mu[7:10], mu_exp)
model.MuMan[13:17, :] = expmap(model.Mu[10:13], mu_exp)
model.MuMan[17:21, :] = expmap(model.Mu[13:16], mu_exp)
model.MuMan[21:25, :] = expmap(model.Mu[16:19], mu_exp)
model.MuMan[25:29, :] = expmap(model.Mu[19:22], mu_exp)
model.MuMan[29:33, :] = expmap(model.Mu[22:25], mu_exp)
model.MuMan[33:37, :] = expmap(model.Mu[25:28], mu_exp)
model.MuMan[37:55, :] = model.Mu[28:46, :]
# Mu
model.Mu = np.zeros((model.nbVar, model.nbStates)) ## ???
uTmp = np.zeros((model.nbVar, nbData * nbSamples, model.nbStates))
avg_loglik = np.zeros(nbIterEM)
Data_gaussPDF = np.zeros((model.nbVar, xIn.size)) ## or xIn.shape[1]
U0 = np.zeros((model.nbVar, model.nbVar, model.nbStates),
dtype=complex) ## U0 is complex
for nb in range(nbIterEM):
L = np.zeros((model.nbStates, model.x.shape[1]))
for i in range(model.nbStates):
Data_gaussPDF[0, :] = xIn - model.MuMan[0, i]
Data_gaussPDF[1:4, :] = logmap(xOut[1], model.MuMan[1:5, i])
Data_gaussPDF[4:7, :] = logmap(xOut[2], model.MuMan[5:9, i])
Data_gaussPDF[7:10, :] = logmap(xOut[3], model.MuMan[9:13, i])
Data_gaussPDF[10:13, :] = logmap(xOut[4], model.MuMan[13:17, i])
Data_gaussPDF[13:16, :] = logmap(xOut[5], model.MuMan[17:21, i])
Data_gaussPDF[16:19, :] = logmap(xOut[6], model.MuMan[21:25, i])
Data_gaussPDF[19:22, :] = logmap(xOut[7], model.MuMan[25:29, i])
Data_gaussPDF[22:25, :] = logmap(xOut[8], model.MuMan[29:33, i])
Data_gaussPDF[25:28, :] = logmap(xOut[9], model.MuMan[33:37, i])
Data_gaussPDF[28:31, :] = xOut[10] - model.MuMan[37:40, i:i + 1]
Data_gaussPDF[31:34, :] = xOut[11] - model.MuMan[40:43, i:i + 1]
Data_gaussPDF[34:37, :] = xOut[12] - model.MuMan[43:46, i:i + 1]
Data_gaussPDF[37:40, :] = xOut[13] - model.MuMan[46:49, i:i + 1]
Data_gaussPDF[40:43, :] = xOut[14] - model.MuMan[49:52, i:i + 1]
Data_gaussPDF[43:46, :] = xOut[15] - model.MuMan[52:55, i:i + 1]
L[i, :] = model.Priors[i] * gaussPDF(Data_gaussPDF, model.Mu[:, i],
model.Sigma[:, :, i])
GAMMA = L / (np.sum(L, axis=0, keepdims=True) + np.finfo(float).tiny)
GAMMA2 = GAMMA / np.sum(GAMMA, axis=1, keepdims=True)
GAMMA2 = np.where(np.isnan(GAMMA2), 0, GAMMA2)
avg_loglik[nb] = -np.log(np.mean(np.sum(L, axis=0)))
# M-step
for i in range(model.nbStates):
# Update Priors
model.Priors[i] = np.sum(GAMMA[i, :]) / (nbData * nbSamples)
# Update MuMan
for n in range(nbIter):
uTmp[0, :, i] = xIn - model.MuMan[0, i]
uTmp[1:4, :, i] = logmap(xOut[1], model.MuMan[1:5, i])
uTmp[4:7, :, i] = logmap(xOut[2], model.MuMan[5:9, i])
uTmp[7:10, :, i] = logmap(xOut[3], model.MuMan[9:13, i])
uTmp[10:13, :, i] = logmap(xOut[4], model.MuMan[13:17, i])
uTmp[13:16, :, i] = logmap(xOut[5], model.MuMan[17:21, i])
uTmp[16:19, :, i] = logmap(xOut[6], model.MuMan[21:25, i])
uTmp[19:22, :, i] = logmap(xOut[7], model.MuMan[25:29, i])
uTmp[22:25, :, i] = logmap(xOut[8], model.MuMan[29:33, i])
uTmp[25:28, :, i] = logmap(xOut[9], model.MuMan[33:37, i])
uTmp[28:31, :, i] = xOut[10] - model.MuMan[37:40, i:i + 1]
uTmp[31:34, :, i] = xOut[11] - model.MuMan[40:43, i:i + 1]
uTmp[34:37, :, i] = xOut[12] - model.MuMan[43:46, i:i + 1]
uTmp[37:40, :, i] = xOut[13] - model.MuMan[46:49, i:i + 1]
uTmp[40:43, :, i] = xOut[14] - model.MuMan[49:52, i:i + 1]
uTmp[43:46, :, i] = xOut[15] - model.MuMan[52:55, i:i + 1]
GAMMA2_col = GAMMA2[i:i + 1, :].T
model.MuMan[0, i] = np.dot(model.MuMan[0, i] + uTmp[0, :, i],
GAMMA2_col)
model.MuMan[1:5, i:i + 1] = expmap(
np.dot(uTmp[1:4, :, i], GAMMA2_col), model.MuMan[1:5, i])
model.MuMan[5:9, i:i + 1] = expmap(
np.dot(uTmp[4:7, :, i], GAMMA2_col), model.MuMan[5:9, i])
model.MuMan[9:13, i:i + 1] = expmap(
np.dot(uTmp[7:10, :, i], GAMMA2_col), model.MuMan[9:13, i])
model.MuMan[13:17, i:i + 1] = expmap(
np.dot(uTmp[10:13, :, i], GAMMA2_col), model.MuMan[13:17,
i])
model.MuMan[17:21, i:i + 1] = expmap(
np.dot(uTmp[13:16, :, i], GAMMA2_col), model.MuMan[17:21,
i])
model.MuMan[21:25, i:i + 1] = expmap(
np.dot(uTmp[16:19, :, i], GAMMA2_col), model.MuMan[21:25,
i])
model.MuMan[25:29, i:i + 1] = expmap(
np.dot(uTmp[19:22, :, i], GAMMA2_col), model.MuMan[25:29,
i])
model.MuMan[29:33, i:i + 1] = expmap(
np.dot(uTmp[22:25, :, i], GAMMA2_col), model.MuMan[29:33,
i])
model.MuMan[33:37, i:i + 1] = expmap(
np.dot(uTmp[25:28, :, i], GAMMA2_col), model.MuMan[33:37,
i])
model.MuMan[37:40, i:i + 1] = np.dot(
model.MuMan[37:40, i:i + 1] + uTmp[28:31, :, i],
GAMMA2_col)
model.MuMan[40:43, i:i + 1] = np.dot(
model.MuMan[40:43, i:i + 1] + uTmp[31:34, :, i],
GAMMA2_col)
model.MuMan[43:46, i:i + 1] = np.dot(
model.MuMan[43:46, i:i + 1] + uTmp[34:37, :, i],
GAMMA2_col)
model.MuMan[46:49, i:i + 1] = np.dot(
model.MuMan[46:49, i:i + 1] + uTmp[37:40, :, i],
GAMMA2_col)
model.MuMan[49:52, i:i + 1] = np.dot(
model.MuMan[49:52, i:i + 1] + uTmp[40:43, :, i],
GAMMA2_col)
model.MuMan[52:55, i:i + 1] = np.dot(
model.MuMan[52:55, i:i + 1] + uTmp[43:46, :, i],
GAMMA2_col)
# Update Sigma
model.Sigma[:, :, i] = uTmp[:, :, i].dot(np.diag(
GAMMA2[i, :])).dot(uTmp[:, :, i].T) + np.identity(
u.shape[0]) * model.params_diagRegFact
# Eigendecomposition of Sigma
for i in range(model.nbStates):
W, V = np.linalg.eig(
model.Sigma[:, :, i]
) ##np.linalg.eig doesn't return exactly the same result as eig() in matlab
D_sqrt = np.diag(np.sqrt(W))
U0[:, :, i] = np.dot(V, D_sqrt)
model.U0 = U0
pass