Python Quaternion类代码示例

 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()

def trinterp(T0, T1, r):
    """
    Interpolate homogeneous transformations.
    Compute a homogeneous transform interpolation between C{T0} and C{T1} as
    C{r} varies from 0 to 1 such that::
    
        trinterp(T0, T1, 0) = T0
        trinterp(T0, T1, 1) = T1
        
    Rotation is interpolated using quaternion spherical linear interpolation.

    @type T0: 4x4 homogeneous transform
    @param T0: Initial value
    @type T1: 4x4 homogeneous transform
    @param T1: Final value
    @type r: number
    @param r: Interpolation index, in the range 0 to 1 inclusive
    @rtype: 4x4 homogeneous transform
    @return: Interpolated value
    @see: L{quaternion}, L{ctraj}
    """
    
    q0 = Quaternion(T0)
    q1 = Quaternion(T1)
    p0 = transl(T0)
    p1 = transl(T1)


    qr = q0.interp(q1, r)
    pr = p0*(1-r) + r*p1

    return vstack( (concatenate((qr.R(),pr),1), mat([0,0,0,1])) )

 def applyBodyTransformToTwist(self, velID, rorID, translation, rotation):
     newVel = Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),rotation),
                                  self.cols(velID) \
                                  + np.cross(self.cols(rorID),np.kron(np.ones([self.length(),1]),translation)))
     newRor = Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),rotation),
                                  self.cols(rorID))
     for i in np.arange(0,3):
         self.setCol(newVel[:,i],velID[i])
         self.setCol(newRor[:,i],rorID[i])

 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])

 def alignBodyFrame(self):
     if(self.extraTransformPos != None or self.extraTransformAtt != None):
         if(self.alignMode == 0 or self.alignMode == 2):
             self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
         if(self.alignMode == 1 or self.alignMode == 3):
             self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
             if(self.doCov):
                 self.td.applyBodyTransformToAttCov('attCov', self.extraTransformAtt)
                 if((self.extraTransformPos != np.array([0.0, 0.0, 0.0])).any()):
                     print('Warning: Covariance are not properly mapped if there is a translational component on the Body transform')
     if(self.alignMode == 0):
         B_r_BC_est, qCB_est = self.td.calibrateBodyTransform('vel', 'ror', self.tdgt, 'vel','ror')
         print('Align Body Transform (estimate to GT):')
         print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
         print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
         self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
         if(self.doCov):
             print('Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction')
     if(self.alignMode == 1):
         B_r_BC_est, qCB_est = self.tdgt.calibrateBodyTransform('vel', 'ror', self.td, 'vel','ror')
         print('Align Body Transform (GT to estimate):')
         print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
         print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
         self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
     if(self.alignMode == 2):
         B_r_BC_est = self.MrMV
         qCB_est = self.qVM
         print('Fixed Body Alignment (estimate to GT):')
         print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
         print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
         self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
         if(self.doCov):
             print('Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction')
         if(self.extraTransformPos != None or self.extraTransformAtt != None):
             self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
             if(self.doCov):
                 self.td.applyBodyTransformToAttCov('attCov', self.extraTransformAtt)
                 if(self.extraTransformPos != np.array([0.0, 0.0, 0.0])):
                     print('Warning: Covariance are not properly mapped if there is a translational component on the Body transform')
     if(self.alignMode == 3):
         B_r_BC_est = -Quaternion.q_rotate(self.qVM, self.MrMV)
         qCB_est = Quaternion.q_inverse(self.qVM)
         print('Fixed Body Alignment (GT to estimate):')
         print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
         print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
         self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
         if(self.extraTransformPos != None or self.extraTransformAtt != None):
             self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)

 def computeRotationalRateFromAttitude(self, attitudeID, rotationalrateID, a=1, b=0):
     dv = Quaternion.q_boxMinus(self.d[a+b:self.end(),attitudeID],self.d[0:self.end()-(a+b),attitudeID])
     dt = self.getTime()[a+b:self.end()]-self.getTime()[0:self.end()-(a+b)]
     for i in np.arange(0,3):
         self.d[0:a,rotationalrateID[i]].fill(0)
         self.d[self.end()-b:self.end(),rotationalrateID[i]].fill(0)
         self.d[a:self.end()-b,rotationalrateID[i]] = np.divide(dv[:,i],dt);

 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])

def insertVisual(parentNode, solid, color):
    node = parentNode.createChild("node_"+solid.name)
    translation=solid.position[:3]
    rotation = Quaternion.to_euler(solid.position[3:])  * 180.0 / math.pi
    for m in solid.mesh:
        Tools.meshLoader(node, m.source, name="loader_"+m.id)
        node.createObject("OglModel",src="@loader_"+m.id, translation=concat(translation),rotation=concat(rotation), color=color)

 def invertTransform(self, pos, att):
     posID = self.getColIDs(pos)
     attID = self.getColIDs(att)
     newTranslation = -Quaternion.q_rotate(self.col(attID),self.col(posID))
     for i in np.arange(0,3):
         self.setCol(newTranslation[:,i],posID[i])
     self.invertRotation(attID)

 def quaternionToYprCov(self, att, attCov, yprCov):
     attIDs = self.getColIDs(att)
     attCovIDs = self.getColIDs(attCov)
     yprCovIDs = self.getColIDs(yprCov)
     J = Quaternion.q_toYprJac(self.col(attIDs))
     # Do the multiplication by hand, improve if possible
     for i in xrange(0,self.length()):
         self.d[i,yprCovIDs] = np.resize(np.dot(np.dot(np.resize(J[i,:],(3,3)),np.resize(self.col(attCovIDs)[i,:],(3,3))),np.resize(J[i,:],(3,3)).T),(9))

 def computeRotationalRateFromAttitude(self, colAtt, colRor, a=1, b=0):
     colAttIDs = self.getColIDs(colAtt)
     colRorIDs = self.getColIDs(colRor)
     dv = Quaternion.q_boxMinus(self.d[a+b:self.end(),colAttIDs],self.d[0:self.end()-(a+b),colAttIDs])
     dt = self.getTime()[a+b:self.end()]-self.getTime()[0:self.end()-(a+b)]
     for i in np.arange(0,3):
         self.d[0:a,colRorIDs[i]].fill(0)
         self.d[self.end()-b:self.end(),colRorIDs[i]].fill(0)
         self.d[a:self.end()-b,colRorIDs[i]] = np.divide(dv[:,i],dt);

 def applyRotationToCov(self, cov, att, doInverse=False):
     covIDs = self.getColIDs(cov)
     attIDs = self.getColIDs(att)
     # Do the multiplication by hand, improve if possible
     for i in xrange(0,self.length()):
         R = np.resize(Quaternion.q_toRotMat(self.d[i,attIDs]),(3,3))
         if(doInverse):
             self.d[i,covIDs] = np.resize(np.dot(np.dot(R.T,np.resize(self.d[i,covIDs],(3,3))),R),(9))
         else:
             self.d[i,covIDs] = np.resize(np.dot(np.dot(R,np.resize(self.d[i,covIDs],(3,3))),R.T),(9))

 def calibrateBodyTransform(self, vel1, ror1, other, vel2, ror2):
     velID1 = self.getColIDs(vel1)
     rorID1 = self.getColIDs(ror1)
     velID2 = other.getColIDs(vel2)
     rorID2 = other.getColIDs(ror2)
     # 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(7);
     td2 = TimedData(7);
     td1.initEmptyFromTimes(np.arange(first,last,timeIncrement))
     td2.initEmptyFromTimes(np.arange(first,last,timeIncrement))
     self.interpolateColumns(td1, velID1, [1,2,3])
     self.interpolateColumns(td1, rorID1, [4,5,6])
     other.interpolateColumns(td2, velID2, [1,2,3])
     other.interpolateColumns(td2, rorID2, [4,5,6])
     AA = np.zeros([4,4])
     # TODO: Speed up by removing for loop
     for i in np.arange(0,td1.length()):
         q1 = np.zeros(4)
         q2 = np.zeros(4)
         q1[1:4] = td1.D()[i,4:7];
         q2[1:4] = td2.D()[i,4:7];
         A = Quaternion.q_Rmat(q1)-Quaternion.q_Lmat(q2)
         AA += A.T.dot(A)
     w, v = np.linalg.eigh(AA)
     rotation = v[:,0]
     
     # Find transformation
     A = np.zeros([3*td1.length(),3])
     b = np.zeros([3*td1.length()])
     for i in np.arange(0,td1.length()):
         A[3*i:3*i+3,] = np.resize(Utils.skew(td1.D()[i,4:7]),(3,3))
         b[3*i:3*i+3] = Quaternion.q_rotate(Quaternion.q_inverse(rotation), td2.D()[i,1:4])-td1.D()[i,1:4]
     translation = np.linalg.lstsq(A, b)[0]
     
     return translation, rotation

def calc_vis_values(iproc, ephem_Times, chandra_ecis, q1s, q2s, q3s, q4s):
    outvals = []
    for t, chandra_eci, q1, q2, q3, q4 in zip(ephem_Times, chandra_ecis, q1s, q2s, q3s, q4s):
        alt = np.sqrt(np.sum(chandra_eci**2))/1e3
        date = re.sub(r'\.000$', '', DateTime(t).date)
        q_att = Quaternion.normalize([q1,q2,q3,q4])
        vis, illum, rays = taco.calc_earth_vis(chandra_eci, q_att, ngrid=opt.ngrid)
        title = '%s alt=%6.0f illum=%6.4f' % (date, alt, illum)
        outvals.append((t, illum, alt, q1, q2, q3, q4))
        if opt.verbose:
            print title, taco.norm(chandra_eci), q1, q2, q3, q4
        elif iproc == 0:
            print 'Progress: %d%%\r' % int((100. * len(outvals)) / len(ephem_Times) + 0.5),
            sys.stdout.flush()

 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