def importPostProcessing(dvs, samples, dvsLbl=None, dvsFlow=None, **kwargs):
'''
Split by channel: The iit yarp format assumes that the channel bit
corresponds to 'left' and 'right' sensors, so it's handled explicitly here
'''
channels = {}
for ch in [0, 1]:
chDict = {}
dvsCh = selectByLabel(dvs, 'ch', ch)
if dvsCh:
chDict['dvs'] = dvsCh
if dvsLbl:
dvsLblCh = selectByLabel(dvsLbl, 'ch', ch)
if dvsLblCh:
chDict['dvslbl'] = dvsLblCh
samplesCh = selectByLabel(samples, 'ch', ch)
if samplesCh:
if kwargs.get('convertSamplesToImu', True):
chDict['imu'] = samplesToImu(samplesCh, **kwargs)
else:
chDict['sample'] = samplesCh
if dvsFlow:
dvsFlowCh = selectByLabel(dvsFlow, 'ch', ch)
if dvsFlowCh:
chDict['flow'] = dvsFlowCh
chDict['flow']['vx'] = flowFromBitsToFloat(
chDict['flow']['vx'])
chDict['flow']['vy'] = flowFromBitsToFloat(
chDict['flow']['vy'])
if any(chDict):
for dataType in chDict:
chDict[dataType]['ts'] = unwrapTimestamps(
chDict[dataType]['ts'], **
kwargs) * 0.00000008 # Convert to seconds
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
zeroTimestampsForAChannel(chDict)
if ch == 0:
channels['left'] = chDict
else:
channels['right'] = chDict
# Construct the output dict
outDict = {'info': kwargs, 'data': channels}
outDict['info']['fileFormat'] = 'iityarp'
return outDict
def interpretMsgsAsPose6qTransform(msgs, **kwargs):
'''
Hmm - it's a bit hack how this is stuck in here at the moment,
but whereas rpg used PoseStamped message type, realsense use Transform:
ros message is defined here:
http://docs.ros.org/api/geometry_msgs/html/msg/Transform.html
the result is a 7-column np array of np.float64,
where the cols are x, y, z, q-x, q-y, q-z, q-w (i.e. quaternion orientation)
ts must therefore come from the message header; does that mean there is
only one pose per message?
'''
numEvents = 0
sizeOfArray = 1024
tsAll = np.zeros((sizeOfArray), dtype=np.float64)
poseAll = np.zeros((sizeOfArray, 7), dtype=np.float64)
for msg in tqdm(msgs, position=0, leave=True):
if sizeOfArray < numEvents + 1:
tsAll = np.append(tsAll, np.zeros((sizeOfArray), dtype=np.float64))
poseAll = np.concatenate(
(poseAll, np.zeros((sizeOfArray, 7), dtype=np.float64)))
sizeOfArray *= 2
# Note - ignoring kwargs['useRosMsgTimestamps'] as there is no choice
timeS, timeNs = unpack('=LL', msg['time'])
tsAll[numEvents] = np.float64(timeS) + np.float64(timeNs) * 0.000000001
poseAll[numEvents, :] = np.frombuffer(msg['data'], dtype=np.float64)
numEvents += 1
# Crop arrays to number of events
tsAll = tsAll[:numEvents]
tsUnwrapped = unwrapTimestamps(
tsAll
) # here we could pass in wrapTime=2**62, but actually it handles this internally
poseAll = poseAll[:numEvents]
point = poseAll[:, 0:3]
rotation = poseAll[:, [6, 3, 4,
5]] # Switch quaternion form from xyzw to wxyz
outDict = {'ts': tsUnwrapped, 'point': point, 'rotation': rotation}
return outDict
def importSecDvs(**kwargs):
filePathOrName = kwargs['filePathOrName']
print('Attempting to import ' + filePathOrName + ' as secdvs')
with open(filePathOrName, 'rb') as file:
data = np.fromfile(file, dtype='>u4')
print('Clipping any rows without column info...')
# Hunt for the first col iteratively and cut data before this
for idx, word in enumerate(data):
if word & 0x4000000:
break
data = data[idx:]
print('Building indices for word types ...')
isTs = (data & 0x08000000).astype(np.bool) # Reference timestamp, i.e. tsMsb
isCol = (data & 0x4000000).astype(np.bool) # X and tsLsb
isRow = (data & 0x80000000).astype(np.bool) # y and pol
print('Handling col data ...')
# create an index which points data back to col
numCol = np.count_nonzero(isCol)
colIdsRange = np.arange(numCol)
colIdsSparse = np.where(isCol)[0]
colIdsNext = np.append(colIdsSparse[1:], len(data))
colIdx = np.zeros_like(data)
for rangeIdx, firstIdx, nextIdx in zip(colIdsRange, colIdsSparse, colIdsNext):
colIdx[firstIdx:nextIdx] = rangeIdx
# now we can isolate col
col = data[isCol]
# convert col data
xByCol = (col & 0x000003FF).astype(np.uint16) # column Address
tsSmallByCol = (col & 0x1FF800) >> 11
# create col data vectors which match data
x = xByCol[colIdx]
tsSmall = tsSmallByCol[colIdx]
print('Handling timestamp data ...')
# from data extract the "start col" flag
isStartCol = (data & 0x200000).astype(np.bool) & isCol
# now, for each startCol, we want to search backwards through data for a ts
# To do this, we find the "data" idx of start col, we create a set of tsIds
# and then we search tsIds for each dataIdx
tsIdsSparse = np.where(isTs)[0]
# Actually find the tsMsb at this point, to avoid more indexing
tsMsbSparse = (data[isTs] & 0x003FFFFF) << 10
tsMsbSparse = np.insert(tsMsbSparse, 0, tsMsbSparse[0] - (1 << 10))
# create an index which points data back to startCols
startColIdsSparse = np.where(isStartCol)[0]
# Search tsIds for each dataIdx
tsForStartCol = np.zeros_like(startColIdsSparse)
for idx, startColIdx in enumerate(startColIdsSparse):
tsForStartCol[idx] = tsMsbSparse[np.searchsorted(tsIdsSparse, startColIdx)]
# Now we have the ts(Large) for each startCol event. Now create a tsLarge
# array which matches data; do this in just the same way as above for
# tsSmall given col and colIdx
# create an index which points data back to col
numStartCol = np.count_nonzero(isStartCol)
startColIdsRange = np.arange(numStartCol)
startColIdsSparse = np.where(isStartCol)[0]
startColIdsNext = np.append(startColIdsSparse[1:], len(data))
startColIdx = np.zeros_like(data)
for rangeIdx, firstIdx, nextIdx in zip(startColIdsRange, startColIdsSparse, startColIdsNext):
startColIdx[firstIdx:nextIdx] = rangeIdx
tsLarge = tsForStartCol[startColIdx]
# Now we have tsLarge and tsSmall aligned by data,
# we can sum these to give the full ts
ts = tsLarge + tsSmall
print('Handling row data ...')
# Now we have x and ts for each row in data;
# now select these just for group/row data
x = x[isRow]
ts = ts[isRow]
data = data[isRow]
# A major timewrap is possible, so handle unwrapping before the following
# processing, which will mix up the timestamps
ts = unwrapTimestamps(ts) / 1000000 # Convert to seconds in the same step
# Break out addr and pol for each of the two groups
pol1 = ((data & 0x00010000) >> 16).astype(np.bool)
yLarge1 = ((data & 0x00FC0000) >> 15).astype(np.uint16) # grp1Address
pol2 = ((data & 0x00020000) >> 17).astype(np.bool)
grp2Offset = (data & 0x7C000000) >> 23
yLarge2 = (grp2Offset + yLarge1).astype(np.uint16)
# for each of the single bit indices, select events for each of the two groups
grp1Events = data & 0xFF
grp2Events = data & 0xFF00
tsToConcatenate = []
xToConcatenate = []
yToConcatenate = []
polToConcatenate = []
for idx in tqdm(range(8)):
#group 1
grp1Bool = (grp1Events & (2 ** idx)).astype(np.bool)
tsToConcatenate.append(ts[grp1Bool])
xToConcatenate.append(x[grp1Bool])
yToConcatenate.append(yLarge1[grp1Bool] + idx)
polToConcatenate.append(pol1[grp1Bool])
# group 2
grp2Bool = (grp2Events & (2 ** (idx + 8))).astype(np.bool)
tsToConcatenate.append(ts[grp2Bool])
xToConcatenate.append(x[grp2Bool])
yToConcatenate.append(yLarge2[grp2Bool] + idx)
polToConcatenate.append(pol2[grp2Bool])
print('Post-processing steps ...')
# Concatenate the resulting arrays
ts = np.concatenate(tsToConcatenate)
x = np.concatenate(xToConcatenate)
y = np.concatenate(yToConcatenate)
pol = np.concatenate(polToConcatenate)
# The above selection strategy mixed up the timestamps, so sort the events by ts
ids = np.argsort(ts)
ts = ts[ids]
x = x[ids]
y = y[ids]
pol = pol[ids]
dvsDict = {'ts': ts,
'x': x,
'y': y,
'pol': pol,
}
if kwargs.get('zeroTime', kwargs.get('zeroTimestamps', True)):
zeroTimestampsForADataType(dvsDict)
outDict = {
'info': {'filePathOrName':filePathOrName,
'fileFormat': 'secdvs'},
'data': {
'ch0': {
'dvs': dvsDict
}
}
}
print('Done.')
return outDict