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')): CsvDataAcquisition.csvLoadTransform(okvisOutputFile, 0, 1, 4, td_okvis, 'pos', 'att') elif (okvisOutputFile.endswith('.bag')): RosDataAcquisition.rosBagLoadOdometry(okvisOutputFile, okvisOutputTopic, td_okvis, 'pos', 'att', 'vel', 'ror') if (okvisOutputFile.endswith('data.csv')): td_okvis.d[:, 0] = td_okvis.d[:, 0] * 1e-9 if (okvisOutputFile.endswith('0.5speed.bag')): td_okvis.d[:, 0] = 0.5 * td_okvis.d[:, 0]
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) if(okvisOutputFile.endswith('data.csv')): td_okvis.d[:,0] = td_okvis.d[:,0]*1e-9 if(okvisOutputFile.endswith('0.5speed.bag')): td_okvis.d[:,0] = 0.5*td_okvis.d[:,0] if(okvisOutputFile == '/home/michael/datasets/long_trajectory_leutiLynen/aslam/aslam_estimator_output_data.csv'): q_real = td_okvis.col(okvis_attID[3]) q_im = -td_okvis.cols(okvis_attID[0:3])
# 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) """ The calibration of the Body Transform needs the velocity and the rotational rate start IDs. """ B_r_BC_est, qCB_est = td1.calibrateBodyTransform(velBIDs1,rorIDs1,td2, velBIDs2,rorIDs2) vCB_est = Quaternion.q_log(qCB_est) vCB_err = vCB-vCB_est B_r_BC_err = B_r_BC - B_r_BC_est print('Calibrate Body Transform:') print('Rotation Vector ln(qCB_est):\tvx:' + str(vCB_est[0]) + '\tvy:' + str(vCB_est[1]) + '\tvz:' + str(vCB_est[2])) print('Translation Vector B_r_BC_est:\trx:' + str(B_r_BC_est[0]) + '\try:' + str(B_r_BC_est[1]) + '\trz:' + str(B_r_BC_est[2])) print('Rotation Error ln(qCB_err):\tvx:' + str(vCB_err[0]) + '\tvy:' + str(vCB_err[1]) + '\tvz:' + str(vCB_err[2])) print('Translation Error B_r_BC_err:\trx:' + str(B_r_BC_err[0]) + '\try:' + str(B_r_BC_err[1]) + '\trz:' + str(B_r_BC_err[2]))
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')): CsvDataAcquisition.csvLoadTransform(okvisOutputFile, 0, 1, 4, td_okvis, 'pos', 'att') elif(okvisOutputFile.endswith('.bag')): RosDataAcquisition.rosBagLoadOdometry(okvisOutputFile,okvisOutputTopic,td_okvis,'pos','att','vel','ror') if(okvisOutputFile.endswith('data.csv')): td_okvis.d[:,0] = td_okvis.d[:,0]*1e-9 if(okvisOutputFile.endswith('0.5speed.bag')): td_okvis.d[:,0] = 0.5*td_okvis.d[:,0] if(okvisOutputFile == '/home/michael/datasets/long_trajectory_leutiLynen/aslam/aslam_estimator_output_data.csv'): q_real = td_okvis.col('att'[3]) q_im = -td_okvis.col('att'[0:3])