Exemplo n.º 1
0
if True:  # Vicon data acquisition and pre-processing
    if (viconGroundtruthFile.endswith('.csv')):
        CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4,
                                            td_vicon, 'pos', 'att')
    elif (viconGroundtruthFile.endswith('.bag')):
        RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,
                                                      viconGroundtruthTopic,
                                                      td_vicon, 'pos', 'att')
    if (viconGroundtruthFile.endswith('data.csv')):
        td_vicon.d[:, 0] = td_vicon.d[:, 0] * 1e-9
    td_vicon.cropTimes(td_vicon.getFirstTime() + startcut,
                       td_vicon.getLastTime() - endcut)
    td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
    td_vicon.computeRotationalRateFromAttitude('att', 'ror', 3, 3)
    td_vicon.computeNormOfColumns('ror', 'ron')
    td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame(
        'pos', 'vel', 'att', 3, 3)

if doRovio:  # Rovio data acquisition and pre-processing
    RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic,
                                          td_rovio, 'pos', 'att', 'vel', 'ror',
                                          'posCov', 'attCov')
    if rovioPclTopic != '':
        RosDataAcquisition.rosBagLoadRobocentricPointCloud(
            rovioOutputBag, rovioPclTopic, td_rovio, 'feaIdx', 'feaPos',
            'feaCov')
    if rovioExtrinsicTopic != '':
        RosDataAcquisition.rosBagLoadPoseWithCovariance(
            rovioOutputBag, rovioExtrinsicTopic, td_rovio, 'extPos', 'extAtt',
            'extPosCov', 'extAttCov')
    td_rovio.computeNormOfColumns('ror', 'ron')
    td_rovio.applyTimeOffset(td_vicon.getFirstTime() - td_rovio.getFirstTime())
Exemplo n.º 2
0
    okvis_rorID = [11,12,13]
    okvis_ronID = 14
    okvis_yprID = [15,16,17]

if True: # Vicon data acquisition and pre-processing
    if(viconGroundtruthFile.endswith('.csv')):
        CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4, td_vicon, vicon_posID, vicon_attID)
    elif(viconGroundtruthFile.endswith('.bag')):
        RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,viconGroundtruthTopic,td_vicon,vicon_posID,vicon_attID)
    if(viconGroundtruthFile.endswith('data.csv')):
        td_vicon.d[:,0] = td_vicon.d[:,0]*1e-9
    td_vicon.cropTimes(td_vicon.getFirstTime()+startcut,td_vicon.getLastTime()-endcut)
    td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
    td_vicon.computeRotationalRateFromAttitude(vicon_attID,vicon_rorID)
    td_vicon.computeNormOfColumns(vicon_rorID,vicon_ronID)
    td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame(vicon_posID, vicon_velID, vicon_attID)

if doRovio: # Rovio data acquisition and pre-processing
    RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic ,td_rovio,rovio_posID,rovio_attID,rovio_velID,rovio_rorID,rovio_posCovID,rovio_attCovID)
    RosDataAcquisition.rosBagLoadRobocentricPointCloud(rovioOutputBag,'/rovio/pcl',td_rovio,rovio_fea_idxID,rovio_fea_posID)
    td_rovio.computeNormOfColumns(rovio_rorID,rovio_ronID)
    td_rovio.applyTimeOffset(td_vicon.getFirstTime()-td_rovio.getFirstTime())
    to = td_rovio.getTimeOffset(rovio_ronID,td_vicon,vicon_ronID)
    td_rovio.applyTimeOffset(-to)
    td_rovio.cropTimes(td_vicon.getFirstTime(),td_vicon.getLastTime())

if doOkvis: # Okvis data acquisition and pre-processing
    if(okvisOutputFile.endswith('.csv')):
        CsvDataAcquisition.csvLoadTransform(okvisOutputFile, 0, 1, 4, td_okvis, okvis_posID, okvis_attID)
    elif(okvisOutputFile.endswith('.bag')):
        RosDataAcquisition.rosBagLoadTransformStamped(okvisOutputFile,okvisOutputTopic,td_okvis,okvis_posID,okvis_attID)
Exemplo n.º 3
0
	print('Applying Time Offset:')
	print('Time Offset: ' + str(timeOffset) + 's')
	
	# Add transformed x to plot
	plotter1.addDataToSubplot(td1, posIDs1[0], 2, 'r', 'td1In x');
	plotter1.addDataToSubplot(td2, posIDs2[0], 2, 'b', 'td2Trans x');
	
	"""
		Now we are ready to calculate the other TimedData properties.
	"""
	# Calculate the velocity in the world frame provide pos(td1=[1,2,3], td2=[9,10,11]) and vel(td1=[8,9,10], td2=[12,13,14]) column IDs.
	td1.computeVectorNDerivative(posIDs1, velIDs1)
	td2.computeVectorNDerivative(posIDs2, velIDs2)
	# Calculate the velocity in the body frame provide pos(td1=[1,2,3], td2=[9,10,11]) and velInBodyFrame(td1=[11,12,13], td2=[15,16,17]) column IDs.
	# Additionally the rotation Quaternion qBI rps qCJ has to be provided.
	td1.computeVelocitiesInBodyFrameFromPostionInWorldFrame(posIDs1, velBIDs1, attIDs1)
	td2.computeVelocitiesInBodyFrameFromPostionInWorldFrame(posIDs2, velBIDs2, attIDs2)
	# Calculate the Rotational Rate provide att(td1=4, td2=1) and rot(td1=14, td2=5) start column IDs.
	td1.computeRotationalRateFromAttitude(attIDs1,rorIDs1)
	td2.computeRotationalRateFromAttitude(attIDs2,rorIDs2)
	# Calculate the Norm of the Rotational Rate provide ror(td1=[14,15,16], td2=[5,6,7]) and rorNorm(td1=17,td2=8) column IDs.
	td1.computeNormOfColumns(rorIDs1,rorNID1)
	td2.computeNormOfColumns(rorIDs2,rorNID2)
	
	"""
		We can estimate the time offset using the norm of the rotational rate.
		The estimated time offset is then applied to td2.
	"""
	to = td2.getTimeOffset(rorNID2,td1,rorNID1)
	td2.applyTimeOffset(-to)
	
    td_okvis.addLabelingIncremental('ron',1)
    td_okvis.addLabelingIncremental('ypr',3)
    td_okvis.reInit()
 
if True: # Vicon data acquisition and pre-processing
    if(viconGroundtruthFile.endswith('.csv')):
        CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4, td_vicon, 'pos', 'att')
    elif(viconGroundtruthFile.endswith('.bag')):
        RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,viconGroundtruthTopic,td_vicon,'pos','att')
    if(viconGroundtruthFile.endswith('data.csv')):
        td_vicon.d[:,0] = td_vicon.d[:,0]*1e-9
    td_vicon.cropTimes(td_vicon.getFirstTime()+startcut,td_vicon.getLastTime()-endcut)
    td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
    td_vicon.computeRotationalRateFromAttitude('att','ror',3,3)
    td_vicon.computeNormOfColumns('ror','ron')
    td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame('pos', 'vel', 'att',3,3)
 
if doRovio: # Rovio data acquisition and pre-processing
    RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic ,td_rovio,'pos','att','vel','ror','posCov','attCov')
    if rovioPclTopic != '':
        RosDataAcquisition.rosBagLoadRobocentricPointCloud(rovioOutputBag,rovioPclTopic,td_rovio,'feaIdx','feaPos','feaCov')
    if rovioExtrinsicTopic != '':
        RosDataAcquisition.rosBagLoadPoseWithCovariance(rovioOutputBag, rovioExtrinsicTopic ,td_rovio,'extPos','extAtt','extPosCov','extAttCov')
    td_rovio.computeNormOfColumns('ror','ron')
    td_rovio.applyTimeOffset(td_vicon.getFirstTime()-td_rovio.getFirstTime())
    to = td_rovio.getTimeOffset('ron',td_vicon,'ron')
    td_rovio.applyTimeOffset(-to)
    td_rovio.cropTimes(td_vicon.getFirstTime(),td_vicon.getLastTime())
 
if doOkvis: # Okvis data acquisition and pre-processing
    if(okvisOutputFile.endswith('.csv')):