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())
Esempio n. 2
0
 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 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 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 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())
 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())
Esempio n. 8
0
    td_vicon.addLabelingIncremental('ypr', 3)
    td_vicon.reInit()

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
    vicon_rorID = [11,12,13]
    vicon_ronID = 14
    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())
    td_vicon.addLabelingIncremental('ypr',3)
    td_vicon.reInit()
 
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 != '':