def acquireData(self): if(self.bag.endswith('.bag')): if self.doCov: RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic ,self.td,'pos','att','vel','ror','posCov','attCov','velCov','rorCov') else: RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic ,self.td,'pos','att','vel','ror') if self.pclTopic != '' and self.doNFeatures > 0: RosDataAcquisition.rosBagLoadRobocentricPointCloud(self.bag,self.pclTopic,self.td,'feaIdx','feaPos','feaCov','feaDis','feaDisCov') if self.extrinsicsTopic != '' and self.doExtrinsics: if self.doCov: RosDataAcquisition.rosBagLoadPoseWithCovariance(self.bag, self.extrinsicsTopic ,self.td,'extPos','extAtt','extPosCov','extAttCov') else: RosDataAcquisition.rosBagLoadPoseWithCovariance(self.bag, self.extrinsicsTopic ,self.td,'extPos','extAtt') if self.biasesTopic != '' and self.doBiases: if self.doCov: RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic ,self.td,'gyb','acb','gybCov','acbCov') else: RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic ,self.td,'gyb','acb') if(self.bag.endswith('data.csv')): CsvDataAcquisition.csvLoadTransform(self.bag, 0, 1, 4, self.td, 'pos', 'att') self.td.d[:,0] = self.td.d[:,0]*1e-9 attID = self.td.getColIDs('att') q_real = self.td.col(attID[3]) q_im = -self.td.col(attID[0:3]) self.td.setCol(q_real,attID[0]) self.td.setCol(q_im,attID[1:4]) self.td.applyTimeOffset(-self.td.getFirstTime())
def acquireData(self): if self.bag.endswith(".bag"): if self.doCov: RosDataAcquisition.rosBagLoadOdometry( self.bag, self.odomTopic, self.td, "pos", "att", "vel", "ror", "posCov", "attCov", "velCov", "rorCov", ) else: RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic, self.td, "pos", "att", "vel", "ror") if self.pclTopic != "" and self.doNFeatures > 0: RosDataAcquisition.rosBagLoadRobocentricPointCloud( self.bag, self.pclTopic, self.td, "feaIdx", "feaPos", "feaCov", "feaDis", "feaDisCov" ) if self.extrinsicsTopic != "" and self.doExtrinsics: if self.doCov: RosDataAcquisition.rosBagLoadPoseWithCovariance( self.bag, self.extrinsicsTopic, self.td, "extPos", "extAtt", "extPosCov", "extAttCov" ) else: RosDataAcquisition.rosBagLoadPoseWithCovariance( self.bag, self.extrinsicsTopic, self.td, "extPos", "extAtt" ) if self.biasesTopic != "" and self.doBiases: if self.doCov: RosDataAcquisition.rosBagLoadImuWithCovariance( self.bag, self.biasesTopic, self.td, "gyb", "acb", "gybCov", "acbCov" ) else: RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic, self.td, "gyb", "acb") if self.bag.endswith("data.csv"): CsvDataAcquisition.csvLoadTransform(self.bag, 0, 1, 4, self.td, "pos", "att") self.td.d[:, 0] = self.td.d[:, 0] * 1e-9 attID = self.td.getColIDs("att") q_real = self.td.col(attID[3]) q_im = -self.td.col(attID[0:3]) self.td.setCol(q_real, attID[0]) self.td.setCol(q_im, attID[1:4]) self.td.applyTimeOffset(-self.td.getFirstTime())
-0.0257744366974, 0.00375618835797, 0.999660727178, ] ) bodyTranslation = np.array([-0.0216401454975, -0.064676986768, 0.00981073058949]) bodyRotation = Quaternion.q_inverse(Quaternion.q_rotMatToQuat(R_BS)) inertialTranslation = None inertialRotation = None writeData = False # Load Odometry Data td.addLabelingIncremental("pos", 3) td.addLabelingIncremental("att", 4) td.reInit() RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, "pos", "att") # Load Timestamps into new td td_aligned.addLabelingIncremental("pos", 3) td_aligned.addLabelingIncremental("att", 4) td_aligned.reInit() RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic, td_aligned) # Interpolate Data td.interpolateColumns(td_aligned, "pos", "pos") td.interpolateQuaternion(td_aligned, "att", "att") # Plotting plotterPos = Plotter(-1, [3, 1], "Position", ["", "", "time[s]"], ["x[m]", "y[m]", "z[m]"], 10000) plotterAtt = Plotter(-1, [4, 1], "Attitude", ["", "", "", "time[s]"], ["w", "x", "y", "z"], 10000) plotterPos.addDataToSubplotMultiple(td, "pos", [1, 2, 3], ["b", "b", "b"], ["", "", ""])
def acquireData(self, csvTimeCol=0, csvPosCol=range(1, 4), csvAttCol=range(4, 8), csvVelCol=None, csvRorCol=None, timescale=1., delimiter=',', start=0): if (self.bag.endswith('.bag')): if self.doCov: RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic, self.td, 'pos', 'att', 'vel', 'ror', 'posCov', 'attCov', 'velCov', 'rorCov', start=start) else: RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic, self.td, 'pos', 'att', 'vel', 'ror', start=start) if self.pclTopic != '' and self.doNFeatures > 0: RosDataAcquisition.rosBagLoadRobocentricPointCloud( self.bag, self.pclTopic, self.td, 'feaIdx', 'feaPos', 'feaCov', 'feaDis', 'feaDisCov') if self.extrinsicsTopic != '' and self.doExtrinsics: if self.doCov: RosDataAcquisition.rosBagLoadPoseWithCovariance( self.bag, self.extrinsicsTopic, self.td, 'extPos', 'extAtt', 'extPosCov', 'extAttCov') else: RosDataAcquisition.rosBagLoadPoseWithCovariance( self.bag, self.extrinsicsTopic, self.td, 'extPos', 'extAtt') if self.biasesTopic != '' and self.doBiases: if self.doCov: RosDataAcquisition.rosBagLoadImuWithCovariance( self.bag, self.biasesTopic, self.td, 'gyb', 'acb', 'gybCov', 'acbCov') else: RosDataAcquisition.rosBagLoadImuWithCovariance( self.bag, self.biasesTopic, self.td, 'gyb', 'acb') if (self.bag.endswith('.csv')): CsvDataAcquisition.csvLoadTransform(self.bag, csvTimeCol, csvPosCol, csvAttCol, self.td, 'pos', 'att', csvVelCol, csvRorCol, 'vel', 'ror', delimiter=delimiter, start=start) self.td.d[:, csvTimeCol] = self.td.d[:, csvTimeCol] * timescale # attID = self.td.getColIDs('att') # q_real = self.td.col(attID[3]) # q_im = -self.td.col(attID[0:3]) # self.td.setCol(q_real,attID[0]) # self.td.setCol(q_im,attID[1:4]) # self.td.d = self.td.d[start:, :] # self.td.last = self.td.d.shape[0] - 1 self.td.applyTimeOffset(-self.td.getFirstTime())
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 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) if(okvisOutputFile.endswith('data.csv')): td_okvis.d[:,0] = td_okvis.d[:,0]*1e-9 if(okvisOutputFile.endswith('0.5speed.bag')):
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')): CsvDataAcquisition.csvLoadTransform(okvisOutputFile, 0, 1, 4, td_okvis, 'pos', 'att') elif(okvisOutputFile.endswith('.bag')): RosDataAcquisition.rosBagLoadOdometry(okvisOutputFile,okvisOutputTopic,td_okvis,'pos','att','vel','ror')
0.0148655429818, -0.999880929698, 0.00414029679422, 0.999557249008, 0.0149672133247, 0.025715529948, -0.0257744366974, 0.00375618835797, 0.999660727178 ]) bodyTranslation = np.array( [-0.0216401454975, -0.064676986768, 0.00981073058949]) bodyRotation = Quaternion.q_inverse(Quaternion.q_rotMatToQuat(R_BS)) inertialTranslation = None inertialRotation = None writeData = False # Load Odometry Data td.addLabelingIncremental('pos', 3) td.addLabelingIncremental('att', 4) td.reInit() RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, 'pos', 'att') # Load Timestamps into new td td_aligned.addLabelingIncremental('pos', 3) td_aligned.addLabelingIncremental('att', 4) td_aligned.reInit() RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic, td_aligned) # Interpolate Data td.interpolateColumns(td_aligned, 'pos', 'pos') td.interpolateQuaternion(td_aligned, 'att', 'att') # Plotting plotterPos = Plotter(-1, [3, 1], 'Position', ['', '', 'time[s]'], ['x[m]', 'y[m]', 'z[m]'], 10000)