def importRpgDvsRos(**kwargs):
topics = importRosbag(**kwargs)
template = kwargs.get('template', {})
# The template becomes the data branch of the importedDict
outDict = {'info': kwargs, 'data': {}}
for channelKey in template:
print('Importing for channel : ', channelKey)
channelKeyStripped = str(channelKey).translate(
str.maketrans('', '', string.punctuation))
outDict['data'][channelKeyStripped] = {}
for dataType in template[channelKey]:
topic = template[channelKey][dataType]
topic = topics.pop(topic)
msgs = topic['msgs']
print('Importing for dataType "' + dataType + '"; there are ' +
str(len(msgs)) + ' messages')
if dataType == 'dvs':
# Interpret these messages as EventArray
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsDvs(msgs, **kwargs)
elif dataType == 'pose6q':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsPose6q(msgs, **kwargs)
elif dataType == 'frame':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsFrame(msgs, **kwargs)
elif dataType == 'cam':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsCam(msgs, **kwargs)
elif dataType == 'imu':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsImu(msgs, **kwargs)
else:
print('dataType "', dataType, '" not recognised.')
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
# Optional: start the timestamps at zero for the first event
# This is done collectively for all the concurrent imports
zeroTimestampsForAChannel(outDict['data'][channelKeyStripped])
# jointly rezero for all channels
if kwargs.get('zeroTimestamps', True):
# Optional: start the timestamps at zero for the first event
# This is done collectively for all the concurrent imports
rezeroTimestampsForImportedDicts(outDict)
# report the remaining topics
remainingTopics = topics.keys()
if remainingTopics:
print(
'The following topics are present in the file but were not imported: '
)
for topic in remainingTopics:
print(topic)
# if cam and dvs exist in the same channel, apply height and width to dimY/X
for channelKey in outDict['data'].keys():
utiliseCameraInfoWithinChannel(outDict['data'][channelKey])
return outDict
'''
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 importRpgDvsRos(filePathOrName, **kwargs):
template = kwargs.get('template')
if template == {}: # Just list contents of bag without importing
topics = importRosbag(filePathOrName=filePathOrName, listTopics=True, **kwargs)
return
topics = importRosbag(filePathOrName=filePathOrName, **kwargs)
outDict = {
'info': kwargs,
'data': {}
}
outDict['info']['filePathOrName'] = filePathOrName
if template is None:
for topicLabel in topics.keys():
rosbagType = topics[topicLabel].pop('rosbagType')
bimveeType = bimveeTypeForRosbagType(rosbagType)
if bimveeType is None:
print('Actually, ' + topicLabel + ' has not been imported, because the rosbag message type ' + rosbagType + ' has not been recognised.')
else:
outDict['data'][topicLabel] = {bimveeType: topics[topicLabel]}
else:
# If we get to here then there is a template to parse
# The template becomes the data branch of the importedDict
for channelKey in template.keys():
channelKeyStripped = str(channelKey).translate(str.maketrans('', '', string.punctuation))
outDict['data'][channelKeyStripped] = {}
for dataType in template[channelKey]:
topicLabel = template[channelKey][dataType]
topic = topics.pop(topicLabel)
rosbagType = topic.pop('rosbagType')
bimveeType = bimveeTypeForRosbagType(rosbagType)
if bimveeType != dataType:
print('dataType "', dataType, '" not correctly defined for topic: "', topicLabel, '"')
else:
outDict['data'][channelKeyStripped][dataType] = topic
# Post processing
if kwargs.get('zeroTime', kwargs.get('zeroTimestamps', True)):
# Optional: start the timestamps at zero for the first event
# This is done collectively for all the concurrent imports
for channelKey in outDict['data'].keys():
zeroTimestampsForAChannel(outDict['data'][channelKey])
# jointly rezero for all channels
rezeroTimestampsForImportedDicts(outDict)
return outDict
def importVicon(**kwargs):
filePathOrName = kwargs['filePathOrName']
pattern = re.compile('(\d+) (\d+\.\d+) \((.*)\)')
# yarpBottleTimes = []
outDict = {'info': {'filePathOrName': filePathOrName}, 'data': {}}
separateBodiesAsChannels = kwargs.get('separateBodiesAsChannels', False)
if separateBodiesAsChannels:
uniqueIds = []
else:
poseDict = {'ts': [], 'point': [], 'rotation': [], 'bodyId': []}
with open(filePathOrName, 'r') as file:
print('Found file to read')
line = file.readline()
while line:
found = pattern.search(line.strip())
# yarpBottleTimes.append(float(found.group(2)))
viconData = found.group(3)
bodies = viconData.split(') (')
for body in bodies:
elements = body.split(" ")
bodyId = elements[1].strip('\"')
ts = elements[2]
point = elements[3:6]
# Note: quaternion order is [w,x,y,z] - this is defined by yarp
# IFrameTransform component, so ignore vicon documentation
rotation = elements[6:]
if separateBodiesAsChannels:
try:
poseDict = outDict['data'][bodyId]['pose6q']
except KeyError:
print('KeyError exception.. Creating new key', bodyId)
uniqueIds.append(bodyId)
outDict['data'][bodyId] = {
'pose6q': {
'ts': [],
'point': [],
'rotation': []
}
}
poseDict = outDict['data'][bodyId]['pose6q']
poseDict['ts'].append(ts)
poseDict['point'].append(point)
poseDict['rotation'].append(rotation)
if not separateBodiesAsChannels:
poseDict['bodyId'].append(bodyId.encode('utf-8'))
line = file.readline()
# converting lists of strings to numpy arrays of objects
if separateBodiesAsChannels:
for id in uniqueIds:
outDict['data'][id]['pose6q']['ts'] = np.array(
outDict['data'][id]['pose6q']['ts'], dtype=np.float64)
outDict['data'][id]['pose6q']['point'] = np.array(
outDict['data'][id]['pose6q']['point'], dtype=np.float64)
outDict['data'][id]['pose6q']['rotation'] = np.array(
outDict['data'][id]['pose6q']['rotation'], dtype=np.float64)
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
zeroTimestampsForAChannel(outDict['data'][id])
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
rezeroTimestampsForImportedDicts(outDict)
outDict['info']['uniqueIds'] = uniqueIds
else:
poseDict['ts'] = np.array(poseDict['ts'], dtype=np.float64)
poseDict['point'] = np.array(poseDict['point'], dtype=np.float64)
poseDict['rotation'] = np.array(poseDict['rotation'], dtype=np.float64)
poseDict['bodyId'] = np.array(poseDict['bodyId'])
poseDict['uniqueIds'] = np.unique(poseDict['bodyId'])
if kwargs.get('separateMarkersFromSegments', False):
outDict['data']['vicon'] = separateMarkersFromSegments(poseDict)
else:
outDict['data']['vicon'] = {'pose6q': poseDict}
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
zeroTimestampsForAChannel(
outDict['data']
['vicon']) # TODO: Zeroing in the separated channel case
return outDict