Python Quaternion.q_inverse方法代码示例

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [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_inverse [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_inverse [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_inverse [as 别名]
 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)

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [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_inverse [as 别名]
 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

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [as 别名]
    def doFeatureDepthEvaluation(
        self, figureId=-1, startTime=25.0, plotFeaTimeEnd=3.0, startAverage=1.0, frequency=20.0
    ):
        if self.doNFeatures > 0 and figureId >= -1:
            plt.ion()
            plt.show(block=False)
            if figureId == -1:
                figure()
            elif figureId >= 0:
                figure(figureId)
            x = [[]]
            y = [[]]
            cov = [[]]
            feaIdxID = self.td.getColIDs("feaIdx")
            feaPosID = self.td.getColIDs("feaPos")
            feaCovID = self.td.getColIDs("feaCov")
            for j in np.arange(self.doNFeatures):
                newFea = self.td.col("pos") + Quaternion.q_rotate(
                    Quaternion.q_inverse(self.td.col("att")), self.td.col(feaPosID[j])
                )
                self.td.applyRotationToCov(feaCovID[j], "att", True)
                for i in np.arange(0, 3):
                    self.td.setCol(newFea[:, i], feaPosID[j][i])

                lastStart = 0.0
                lastID = -1
                startID = 0
                for i in np.arange(self.td.length()):
                    if self.td.d[i, self.td.timeID] > startTime:
                        if self.td.d[i, feaIdxID[j]] < 0.0 or self.td.d[i, feaIdxID[j]] != lastID:
                            if len(x[-1]) > 0:
                                x.append([])
                                y.append([])
                                cov.append([])
                        if self.td.d[i, feaIdxID[j]] >= 0.0:
                            if len(x[-1]) == 0:
                                lastStart = self.td.d[i, self.td.timeID]
                                lastID = self.td.d[i, feaIdxID[j]]
                                startID = i
                            x[-1].append(self.td.d[i, self.td.timeID] - lastStart)
                            y[-1].append(self.td.d[i, feaPosID[j][2]])
                            cov[-1].append(sqrt(self.td.d[i, feaCovID[j][8]]))

            axis = subplot(2, 1, 1)
            for i in np.arange(len(x)):
                plot(x[i], y[i], color=Utils.colors["gray"])

            average = 0
            averageCount = 0
            for i in np.arange(len(y)):
                for j in np.arange(int(startAverage * frequency), len(y[i])):
                    average += y[i][j]
                    averageCount += 1
            average = average / averageCount

            means = []
            meanCovs = []
            stds = []
            for j in np.arange(int(plotFeaTimeEnd * frequency)):
                values = []
                covValues = []
                for i in np.arange(len(y)):
                    if len(y[i]) > j:
                        values.append(y[i][j] - average)
                        covValues.append(cov[i][j])
                means.append(mean(values))
                meanCovs.append(mean(covValues))
                stds.append(std(values))
            print("Final standard deviation: " + str(stds[-1]))
            plot(
                np.arange(plotFeaTimeEnd * frequency) / frequency,
                np.ones(plotFeaTimeEnd * frequency) * average,
                color=Utils.colors["blue"],
                lw=3,
                label="Estimated groundtruth",
            )
            plot(
                np.arange(plotFeaTimeEnd * frequency) / frequency,
                average + np.array(means),
                color=Utils.colors["red"],
                lw=3,
                label="Average",
            )
            plot(
                np.arange(plotFeaTimeEnd * frequency) / frequency,
                average + np.array(means) + np.array(stds),
                "--",
                color=Utils.colors["red"],
                lw=3,
                label="Measured standard deviation",
            )
            plot(
                np.arange(plotFeaTimeEnd * frequency) / frequency,
                average + np.array(means) - np.array(stds),
                "--",
                color=Utils.colors["red"],
                lw=3,
            )
            plot(
                np.arange(plotFeaTimeEnd * frequency) / frequency,
#.........这里部分代码省略.........

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [as 别名]

#.........这里部分代码省略.........
         )
         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
             )

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [as 别名]
timestampTopic = "/okvis/okvis_node/okvis_odometry"
R_BS = np.array(
    [
        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)

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [as 别名]
 def invertRotation(self, att):
     attID = self.getColIDs(att)
     newRotation = Quaternion.q_inverse(self.col(attID))
     for i in np.arange(0,4):
         self.setCol(newRotation[:,i],attID[i])

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [as 别名]
    if doOkvis:
        plotterRon.addDataToSubplot(td_okvis, okvis_ronID, 1, 'g', 'okvis rotational rate norm')

if True: # Transform body coordinate frame for better vizualization, TODO: add to config
    if doRovio:
        td_rovio.applyBodyTransform(rovio_posID, rovio_attID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
        td_rovio.applyBodyTransformToAttCov(rovio_attCovID, bodyTransformForBetterPlotRangeAtt)
        td_rovio.applyBodyTransformToTwist(rovio_velID, rovio_rorID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
        td_rovio.applyInertialTransform(rovio_posID, rovio_attID, np.zeros(3), np.array([0,0,0,1]))
    if doOkvis:
        td_okvis.applyBodyTransform(okvis_posID, okvis_attID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
        td_okvis.applyBodyTransformToTwist(okvis_velID, okvis_rorID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
        td_okvis.applyInertialTransform(okvis_posID, okvis_attID, np.zeros(3), np.array([0,0,0,1]))
        
if True: # Align Vicon to Rovio using calibration (body frame)
    B_r_BC_est = -Quaternion.q_rotate(qVM, MrMV) + Quaternion.q_rotate(Quaternion.q_inverse(qVM),bodyTransformForBetterPlotRangePos)
    qCB_est = Quaternion.q_mult(bodyTransformForBetterPlotRangeAtt,Quaternion.q_inverse(qVM))
    td_vicon.applyBodyTransform(vicon_posID, vicon_attID, B_r_BC_est, qCB_est)
    td_vicon.applyBodyTransformToTwist(vicon_velID, vicon_rorID, B_r_BC_est, qCB_est)
 
if doRovio and bodyAlignViconToRovio: # Align Vicon to Rovio (body frame)
    B_r_BC_est, qCB_est = td_vicon.calibrateBodyTransform(vicon_velID, vicon_rorID, td_rovio, rovio_velID,rovio_rorID)
    print('Calibrate Body Transform for Rovio:')
    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]))
    td_vicon.applyBodyTransform(vicon_posID, vicon_attID, B_r_BC_est, qCB_est)
    td_vicon.applyBodyTransformToTwist(vicon_velID, vicon_rorID, B_r_BC_est, qCB_est)

if doOkvis and bodyAlignOkvisToVicon: # Align Okvis to Vicon (body frame)
    B_r_BC_est, qCB_est = td_okvis.calibrateBodyTransform(okvis_velID, okvis_rorID, td_vicon, vicon_velID,vicon_rorID)
    print('Calibrate Body Transform for Okvis:')

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_inverse [as 别名]
 def test_QuaternionClass(self):        
     v1 = np.array([0.1, -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 and RotMat ###########################
     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 RotMat ###########################
     np.testing.assert_almost_equal(Quaternion.q_rotMatToQuat(Quaternion.q_toRotMat(q1)), q1, 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 Exp and RotMat ###########################
     np.testing.assert_almost_equal(Quaternion.q_rotVecToRotMat(v1), Quaternion.q_toRotMat(Quaternion.q_exp(v1)), decimal=7)
     ########################### Testing mult/exp/rotate ###########################
     np.testing.assert_almost_equal(Quaternion.q_mult(q1, Quaternion.q_mult(Quaternion.q_exp(v2),Quaternion.q_inverse(q1))),Quaternion.q_exp(Quaternion.q_rotate(q1, v2)), 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)
     ########################### Testing Jacobian of Exp ###########################
     for i in np.arange(0,3):
         dv1 = np.array([0.0, 0.0, 0.0])
         dv1[i] = 1.0
         epsilon = 1e-6
         q1 = Quaternion.q_exp(v1)
         q1_dist = Quaternion.q_exp(v1+dv1*epsilon)
         dq1_1 = Quaternion.q_boxMinus(q1_dist, q1)/epsilon
         J = np.resize(Quaternion.q_rotVecToGamma(v1),(3,3))
         dq1_2 = J.dot(dv1)
         np.testing.assert_almost_equal(dq1_1,dq1_2, decimal=5)