plotFeaTimeEnd = 3.0 startTime = 25.0 startAverage = 1.0 frequency = 20.0 figure() x = [[]] y = [[]] 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 plotFea: # Plotting features height, TODO: zoom in & plot vs traveled distance if doRovio: plotFeaTimeEnd = 3.0 startTime = 25.0 startAverage = 1.0 frequency = 20.0 figure() x = [[]] y = [[]] 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: