Python Quaternion.q_boxMinus方法代码示例

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

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

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

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