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 != '':
td_okvis = TimedData(18) okvis_posID = [1,2,3] okvis_attID = [4,5,6,7] okvis_velID = [8,9,10] 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