def acquireDataGT(self): if(self.gtFile.endswith('.csv')): CsvDataAcquisition.csvLoadTransform(self.gtFile, 0, 1, 4, self.tdgt, 'pos', 'att') elif(self.gtFile.endswith('.bag')): RosDataAcquisition.rosBagLoadTransformStamped(self.gtFile,self.gtTopic,self.tdgt,'pos','att') if(self.gtFile.endswith('data.csv')): self.tdgt.d[:,0] = self.tdgt.d[:,0]*1e-9 self.tdgt.cropTimes(self.tdgt.getFirstTime()+self.startcut,self.tdgt.getLastTime()-self.endcut) self.tdgt.applyTimeOffset(-self.tdgt.getFirstTime())
def acquireDataGT(self): if self.gtFile.endswith(".csv"): CsvDataAcquisition.csvLoadTransform(self.gtFile, 0, 1, 4, self.tdgt, "pos", "att") elif self.gtFile.endswith(".bag"): RosDataAcquisition.rosBagLoadTransformStamped(self.gtFile, self.gtTopic, self.tdgt, "pos", "att") if self.gtFile.endswith("data.csv"): self.tdgt.d[:, 0] = self.tdgt.d[:, 0] * 1e-9 self.tdgt.cropTimes(self.tdgt.getFirstTime() + self.startcut, self.tdgt.getLastTime() - self.endcut) self.tdgt.applyTimeOffset(-self.tdgt.getFirstTime())
def acquireDataGT(self, csvTimeCol=0, csvPosCol=range(1, 4), csvAttCol=range(4, 8), timescale=1.): if (self.gtFile.endswith('.csv')): CsvDataAcquisition.csvLoadTransform(self.gtFile, csvTimeCol, csvPosCol, csvAttCol, self.tdgt, 'pos', 'att') self.tdgt.d[:, csvTimeCol] = self.tdgt.d[:, csvTimeCol] * timescale elif (self.gtFile.endswith('.bag')): RosDataAcquisition.rosBagLoadTransformStamped( self.gtFile, self.gtTopic, self.tdgt, 'pos', 'att') if (self.gtFile.endswith('data.csv')): self.tdgt.d[:, 0] = self.tdgt.d[:, 0] * timescale self.tdgt.cropTimes(self.tdgt.getFirstTime() + self.startcut, self.tdgt.getLastTime() - self.endcut) self.tdgt.applyTimeOffset(-self.tdgt.getFirstTime())
td_okvis = TimedData() td_okvis.addLabelingIncremental('pos', 3) td_okvis.addLabelingIncremental('att', 4) td_okvis.addLabelingIncremental('vel', 3) td_okvis.addLabelingIncremental('ror', 3) 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')
attIDs2 = [1,2,3,4] velIDs1 = [8,9,10] velIDs2 = [12,13,14] velBIDs1 = [11,12,13] velBIDs2 = [15,16,17] rorIDs1 = [14,15,16] rorIDs2 = [5,6,7] rorNID1 = 17 rorNID2 = 8 """ First we will fill the two TimedData structures with the same StampedTransforms, loaded from topic 'rovio/transform' in example.bag. The indices denote the start column of the position(td1=1, td2=9) and attitude(td1=4, td2=1) """ RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td1,posIDs1,attIDs1) RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td2,posIDs2,attIDs2) # Add initial x to plot plotter1.addDataToSubplot(td1, posIDs1[0], 1, 'r', 'td1In x'); plotter1.addDataToSubplot(td2, posIDs2[0], 1, 'b', 'td2In x'); """ Apply body transform to the second data set. The column start IDs of position(9) and attitutde(1) have to be provided. We define the rotation through a rotation vector vCB, the exponential represents the corresponding rotation quaternion qCB. The translation is defined by the translation vector B_r_BC """ vCB = np.array([0.1,0.2,0.32]) qCB = Quaternion.q_exp(vCB) B_r_BC = np.array([1.1,-0.2,0.4])
vicon_yprID = [15,16,17] if doOkvis: # Okvis Timed Data 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)
if doOkvis: # Okvis Timed Data td_okvis = TimedData() td_okvis.addLabelingIncremental('pos',3) td_okvis.addLabelingIncremental('att',4) td_okvis.addLabelingIncremental('vel',3) td_okvis.addLabelingIncremental('ror',3) 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')