cov = [[]]
feaIdxID = td_rovio.getColIDs('feaIdx')
feaPosID = td_rovio.getColIDs('feaPos')
feaCovID = td_rovio.getColIDs('feaCov')
for j in np.arange(nFeatures):
newFea = td_rovio.col('pos') + Quaternion.q_rotate(
Quaternion.q_inverse(td_rovio.col('att')),
td_rovio.col(feaPosID[j]))
td_rovio.applyRotationToCov(feaCovID[j], 'att', True)
for i in np.arange(0, 3):
td_rovio.setCol(newFea[:, i], feaPosID[j][i])
lastStart = 0.0
lastID = -1
startID = 0
for i in np.arange(td_rovio.length()):
if (td_rovio.d[i, td_rovio.timeID] > startTime):
if (td_rovio.d[i, feaIdxID[j]] < 0.0
or td_rovio.d[i, feaIdxID[j]] != lastID):
if len(x[-1]) > 0:
x.append([])
y.append([])
cov.append([])
if (td_rovio.d[i, feaIdxID[j]] >= 0.0):
if len(x[-1]) == 0:
lastStart = td_rovio.d[i, td_rovio.timeID]
lastID = td_rovio.d[i, feaIdxID[j]]
startID = i
x[-1].append(td_rovio.d[i, td_rovio.timeID] -
lastStart)
y[-1].append(td_rovio.d[i, feaPosID[j][2]])
plotterYpr.addDataToSubplotMultiple(td_okvis, okvis_yprID, [1,2,3], ['g','g','g'], ['','',''])
if plotFea: # Plotting rotational rate norm
if doRovio:
figure()
x = [[]]
y = [[]]
for j in np.arange(nFeatures):
newFea = td_rovio.cols(rovio_posID) + Quaternion.q_rotate(Quaternion.q_inverse(td_rovio.cols(rovio_attID)),td_rovio.cols(rovio_fea_posID[j]))
for i in np.arange(0,3):
td_rovio.setCol(newFea[:,i],rovio_fea_posID[j][i])
lastStart = 0.0
lastID = -1
startID = 0
for i in np.arange(td_rovio.length()):
if(td_rovio.d[i,td_rovio.timeID] > 25.0):
if(td_rovio.d[i,rovio_fea_idxID[j]] < 0.0 or td_rovio.d[i,rovio_fea_idxID[j]] != lastID):
if len(x[-1]) > 0:
x.append([])
y.append([])
if(td_rovio.d[i,rovio_fea_idxID[j]] >= 0.0):
if len(x[-1]) == 0:
lastStart = td_rovio.d[i,td_rovio.timeID]
lastID = td_rovio.d[i,rovio_fea_idxID[j]]
startID = i
x[-1].append(td_rovio.d[i,td_rovio.timeID]-lastStart)
y[-1].append(td_rovio.d[i,rovio_fea_posID[j][2]])
for i in np.arange(len(x)):
plot(x[i],y[i],'#999999')
