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