Python Quaternion.q_mult方法代码示例

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_mult [as 别名]
 def test_QuaternionClass(self):        
     v1 = np.array([0.2, 0.2, 0.4])
     v2 = np.array([1, 0, 0])         
     q1 = Quaternion.q_exp(v1)
     q2 = Quaternion.q_exp(v2)
     v=np.array([1, 2, 3])
     # Testing Mult and rotate
     np.testing.assert_almost_equal(Quaternion.q_rotate(Quaternion.q_mult(q1,q2),v), Quaternion.q_rotate(q1,Quaternion.q_rotate(q2,v)), decimal=7)
     np.testing.assert_almost_equal(Quaternion.q_rotate(q1,v2), np.resize(Quaternion.q_toRotMat(q1),(3,3)).dot(v2), decimal=7)
     # Testing Boxplus, Boxminus, Log and Exp
     np.testing.assert_almost_equal(Quaternion.q_boxPlus(q1,Quaternion.q_boxMinus(q2,q1)), q2, decimal=7)
     np.testing.assert_almost_equal(Quaternion.q_log(q1), v1, decimal=7)
     # Testing Lmat and Rmat
     np.testing.assert_almost_equal(Quaternion.q_mult(q1,q2), Quaternion.q_Lmat(q1).dot(q2), decimal=7)
     np.testing.assert_almost_equal(Quaternion.q_mult(q1,q2), Quaternion.q_Rmat(q2).dot(q1), decimal=7)
     # Testing ypr and quat
     roll = 0.2
     pitch = -0.5
     yaw = 2.5
     q_test = Quaternion.q_mult(np.array([np.cos(0.5*pitch), 0, np.sin(0.5*pitch), 0]),np.array([np.cos(0.5*yaw), 0, 0, np.sin(0.5*yaw)]))
     q_test = Quaternion.q_mult(np.array([np.cos(0.5*roll), np.sin(0.5*roll), 0, 0]),q_test)
     np.testing.assert_almost_equal(Quaternion.q_toYpr(q_test), np.array([roll, pitch, yaw]), decimal=7)
     # Testing Jacobian of Ypr
     for i in np.arange(0,3):
         dv1 = np.array([0.0, 0.0, 0.0])
         dv1[i] = 1.0
         epsilon = 1e-6
         ypr1 = Quaternion.q_toYpr(q1)
         ypr1_dist = Quaternion.q_toYpr(Quaternion.q_boxPlus(q1,dv1*epsilon))
         dypr1_1 = (ypr1_dist-ypr1)/epsilon
         J = np.resize(Quaternion.q_toYprJac(q1),(3,3))
         dypr1_2 = J.dot(dv1)
         np.testing.assert_almost_equal(dypr1_1,dypr1_2, decimal=5)

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_mult [as 别名]
 def calibrateInertialTransform(self, pos1, att1, other, pos2, att2, B_r_BC, q_CB, calIDs=[0]):      
     posID1 = self.getColIDs(pos1)
     attID1 = self.getColIDs(att1)
     posID2 = other.getColIDs(pos2)
     attID2 = other.getColIDs(att2)
     # Make timing calculation 
     dt1 = self.getLastTime()-self.getFirstTime()
     dt2 = other.getLastTime()-other.getFirstTime()
     first = max(self.getFirstTime(),other.getFirstTime())
     last = min(self.getLastTime(),other.getLastTime())
     timeIncrement = min(dt1/(self.length()-1), dt2/(other.length()-1))
     td1 = TimedData(8);
     td2 = TimedData(8);
     td1.initEmptyFromTimes(np.arange(first,last,timeIncrement))
     td2.initEmptyFromTimes(np.arange(first,last,timeIncrement))
     self.interpolateColumns(td1, posID1, [1,2,3])
     other.interpolateColumns(td2, posID2, [1,2,3])
     self.interpolateQuaternion(td1, attID1, [4,5,6,7])
     other.interpolateQuaternion(td2, attID2, [4,5,6,7])
     
     if(not calIDs):
         calIDs = range(td1.length())
     newIDs = np.arange(0,Utils.getLen(calIDs))
     q_CB_vec = np.kron(np.ones([Utils.getLen(calIDs),1]),q_CB)
     q_JC_vec = Quaternion.q_inverse(td2.D()[newIDs,4:8])
     q_BI_vec = td1.D()[newIDs,4:8]
     B_r_BC_vec = np.kron(np.ones([Utils.getLen(calIDs),1]),B_r_BC)
     J_r_JC = td2.D()[newIDs,1:4];
     J_r_BC = Quaternion.q_rotate(Quaternion.q_mult(q_JC_vec,q_CB_vec), B_r_BC_vec)
     J_r_IB = Quaternion.q_rotate(Quaternion.q_mult(q_JC_vec,Quaternion.q_mult(q_CB_vec,q_BI_vec)),td1.D()[newIDs,1:4])
     
     rotation = Quaternion.q_mean(Quaternion.q_inverse(Quaternion.q_mult(Quaternion.q_mult(q_JC_vec,q_CB_vec),q_BI_vec)))
     translation = np.mean(J_r_JC-J_r_BC-J_r_IB, axis=0)
     return translation.flatten(), rotation.flatten()

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_mult [as 别名]
 def computeLeutiScore(self, posID1, attID1, velID1, other, posID2, attID2, distances, spacings, start):
     output = np.empty((len(distances),6))
     outputFull = []
     startIndex = np.nonzero(self.getTime() >= start)[0][0]
     
     for j in np.arange(len(distances)):
         tracks = [[startIndex, 0.0]]
         lastAddedStart = 0.0
         posErrors = []
         attErrors = []
     
         other_interp = TimedData(8)
         other_interp.initEmptyFromTimes(self.getTime())
         other.interpolateColumns(other_interp, posID2, [1,2,3])
         other.interpolateQuaternion(other_interp, attID2, [4,5,6,7])
         
         it = startIndex
         while it+1<self.last:
             # Check for new seeds
             if(lastAddedStart>spacings[j]):
                 tracks.append([it, 0.0])
                 lastAddedStart = 0.0
             # Add travelled distance
             d = np.asscalar(Utils.norm(self.d[it,velID1]*(self.d[it+1,0]-self.d[it,0])))
             lastAddedStart += d
             tracks = [[x[0], x[1]+d] for x in tracks]
             # Check if travelled distance large enough
             while len(tracks) > 0 and tracks[0][1] > distances[j]:
                 pos1 = Quaternion.q_rotate(self.d[tracks[0][0],attID1], self.d[it+1,posID1]-self.d[tracks[0][0],posID1])
                 pos2 = Quaternion.q_rotate(other_interp.d[tracks[0][0],[4,5,6,7]], other_interp.d[it+1,[1,2,3]]-other_interp.d[tracks[0][0],[1,2,3]])
                 att1 = Quaternion.q_mult(self.d[it+1,attID1], Quaternion.q_inverse(self.d[tracks[0][0],attID1]))
                 att2 = Quaternion.q_mult(other_interp.d[it+1,[4,5,6,7]], Quaternion.q_inverse(other_interp.d[tracks[0][0],[4,5,6,7]]))
                 posErrors.append(np.asscalar(np.sum((pos2-pos1)**2,axis=-1)))
                 attErrors.append(np.asscalar(np.sum((Quaternion.q_boxMinus(att1,att2))**2,axis=-1)))
                 tracks.pop(0)
             it += 1
         
         N = len(posErrors)
         posErrorRMS = (np.sum(posErrors,axis=-1)/N)**(0.5)
         posErrorMedian = np.median(posErrors)**(0.5)
         posErrorStd = np.std(np.array(posErrors)**(0.5))
         attErrorRMS = (np.sum(attErrors,axis=-1)/N)**(0.5)
         attErrorMedian = np.median(attErrors)**(0.5)
         attErrorStd = np.std(np.array(attErrors)**(0.5))
         print('Evaluated ' + str(N) + ' segments with a travelled distance of ' + str(distances[j]) \
                + ':\n  posRMS of ' + str(posErrorRMS) + ' (Median: ' + str(posErrorMedian) + ', Std: ' + str(posErrorStd) + ')' \
                + '\n  attRMS of ' + str(attErrorRMS) + ' (Median: ' + str(attErrorMedian) + ', Std: ' + str(attErrorStd) + ')')
         output[j,:] = [posErrorRMS, posErrorMedian, posErrorStd, attErrorRMS, attErrorMedian, attErrorStd]
         outputFull.append(np.array(posErrors)**(0.5))
     return outputFull

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_mult [as 别名]
 def applyInertialTransform(self, posID, attID, translation, rotation):
     newTranslation = np.kron(np.ones([self.length(),1]),translation) \
                      + Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),Quaternion.q_inverse(rotation)),
                                            self.cols(posID))
     newRotation = Quaternion.q_mult(self.cols(attID),
                                     np.kron(np.ones([self.length(),1]),rotation))
     for i in np.arange(0,3):
         self.setCol(newTranslation[:,i],posID[i])
     for i in np.arange(0,4):
         self.setCol(newRotation[:,i],attID[i])

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_mult [as 别名]
 def applyBodyTransform(self, pos, att, translation, rotation):
     posID = self.getColIDs(pos)
     attID = self.getColIDs(att)
     if translation == None:
         translation = np.array([0.0, 0.0, 0.0])
     if rotation == None:
         rotation = np.array([1.0, 0, 0, 0])
     newTranslation = self.col(posID) \
                      + Quaternion.q_rotate(Quaternion.q_inverse(self.col(attID)),
                                            np.kron(np.ones([self.length(),1]),translation))
     newRotation = Quaternion.q_mult(np.kron(np.ones([self.length(),1]),rotation),
                                     self.col(attID))
     for i in np.arange(0,3):
         self.setCol(newTranslation[:,i],posID[i])
     for i in np.arange(0,4):
         self.setCol(newRotation[:,i],attID[i])

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_mult [as 别名]
# File names and settings
ID = 0

if ID == 0: # MH_01_easy with OKVIS
    rovioOutputBag = '/home/michael/datasets/euroc/MH_01_easy/rovio/2015-12-18-08-51-27.bag'
    rovioOutputTopic = '/rovio/odometry'
    okvisOutputFile = '/home/michael/datasets/euroc/MH_01_easy/okvis/2015-12-22-15-49-07.bag'
    okvisOutputTopic = '/okvis/okvis_node/okvis_transform'
    viconGroundtruthFile = '/home/michael/datasets/euroc/MH_01_easy/data.csv'
    viconGroundtruthTopic = ''
    startcut = 40
    endcut = 10
    MrMV = np.array([0.0, 0.0, 0.0])
    qVM = np.array([1.0, 0, 0, 0])
    bodyTransformForBetterPlotRangePos = np.zeros(3)
    bodyTransformForBetterPlotRangeAtt = Quaternion.q_mult(np.array([(1-0.6*0.6)**(1./2),0,0.6,0]),np.array([0.0,1.0,0.0,0.0]))
    bodyAlignViconToRovio = False
    bodyAlignOkvisToVicon = False
    doRovio = True
    doOkvis = True
    plotRon = False
    plotAtt = False
    plotPos = True
    plotVel = True
    plotRor = True
    plotYpr = True
    plotFea = False
    plotLeutiDistances = np.arange(1,21)

if ID == 1: # MH_05_difficult
    rovioOutputBag = '/home/michael/datasets/euroc/MH_05_difficult/rovio/2015-12-22-08-53-51.bag'