Python Quaternion.q_exp方法代码示例

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_exp [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_exp [as 别名]
 def computeLeutiScore(self, pos1, att1, vel1, other, pos2, att2, distances, spacings, start):
     posID1 = self.getColIDs(pos1)
     attID1 = self.getColIDs(att1)
     velID1 = self.getColIDs(vel1)
     posID2 = other.getColIDs(pos2)
     attID2 = other.getColIDs(att2)
     output = np.empty((Utils.getLen(distances),6))
     outputPosFull = []
     outputAttFull = []
     outputYawFull = []
     outputInclFull = []
     startIndex = np.nonzero(self.getTime() >= start)[0][0]
     
     for j in np.arange(Utils.getLen(distances)):
         tracks = [[startIndex, 0.0]]
         lastAddedStart = 0.0
         posErrors = []
         attErrors = []
         yawErrors = []
         inclErrors = []
     
         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 Utils.getLen(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 = self.d[it+1,attID1]
                 att2 = Quaternion.q_mult(other_interp.d[it+1,[4,5,6,7]],
                                          Quaternion.q_mult(Quaternion.q_inverse(other_interp.d[tracks[0][0],[4,5,6,7]]),
                                          self.d[tracks[0][0],attID1]))
                 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)))
                 yawError = Quaternion.q_toYpr(Quaternion.q_mult(Quaternion.q_inverse(att2),att1))[2]
                 yawErrors.append(yawError**2)
                 att2_cor = Quaternion.q_mult(att2,Quaternion.q_exp(np.array([0.0,0.0,yawError])))
                 inclErrors.append(np.asscalar(np.sum((Quaternion.q_boxMinus(att1,att2_cor))**2,axis=-1)))
                 tracks.pop(0)
             it += 1
         
         N = Utils.getLen(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))
         yawErrorRMS = (np.sum(yawErrors,axis=-1)/N)**(0.5)
         yawErrorMedian = np.median(yawErrors)**(0.5)
         yawErrorStd = np.std(np.array(yawErrors)**(0.5))
         inclErrorRMS = (np.sum(inclErrors,axis=-1)/N)**(0.5)
         inclErrorMedian = np.median(inclErrors)**(0.5)
         inclErrorStd = np.std(np.array(inclErrors)**(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) + ')' \
                + '\n  yawRMS of ' + str(yawErrorRMS) + ' (Median: ' + str(yawErrorMedian) + ', Std: ' + str(yawErrorStd) + ')' \
                + '\n  inclRMS of ' + str(inclErrorRMS) + ' (Median: ' + str(inclErrorMedian) + ', Std: ' + str(inclErrorStd) + ')')
         output[j,:] = [posErrorRMS, posErrorMedian, posErrorStd, attErrorRMS, attErrorMedian, attErrorStd]
         outputPosFull.append(np.array(posErrors)**(0.5))
         outputAttFull.append(np.array(attErrors)**(0.5))
         outputYawFull.append(np.array(yawErrors)**(0.5))
         outputInclFull.append(np.array(inclErrors)**(0.5))
     return outputPosFull, outputAttFull, outputYawFull, outputInclFull

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_exp [as 别名]
	"""
	RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td1,posIDs1,attIDs1)
	RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td2,posIDs2,attIDs2)
	
	# Add initial x to plot
	plotter1.addDataToSubplot(td1, posIDs1[0], 1, 'r', 'td1In x');
	plotter1.addDataToSubplot(td2, posIDs2[0], 1, 'b', 'td2In x');

	
	"""
		Apply body transform to the second data set. The column start IDs of position(9) and attitutde(1) have to be provided.
		We define the rotation through a rotation vector vCB, the exponential represents the corresponding rotation quaternion qCB.
		The translation is defined by the translation vector B_r_BC
	"""
	vCB = np.array([0.1,0.2,0.32])
	qCB = Quaternion.q_exp(vCB)
	B_r_BC = np.array([1.1,-0.2,0.4])
	td2.applyBodyTransform(posIDs2, attIDs2, B_r_BC, qCB)
	print('Applying Body Transform:')
	print('Rotation Vector ln(qCB):\tvx:' + str(vCB[0]) + '\tvy:' + str(vCB[1]) + '\tvz:' + str(vCB[2]))
	print('Translation Vector B_r_BC:\trx:' + str(B_r_BC[0]) + '\try:' + str(B_r_BC[1]) + '\trz:' + str(B_r_BC[2]))
	
	"""
		Apply inertial transform to the second data set. Same procedure as with the body transform.
	"""
	vIJ = np.array([0.2,-0.2,-0.4])
	qIJ = Quaternion.q_exp(vIJ)
	J_r_JI = np.array([-0.1,0.5,0.1])
	td2.applyInertialTransform(posIDs2, attIDs2,J_r_JI,qIJ)
	print('Applying Inertial Transform:')
	print('Rotation Vector ln(qIJ):\tvx:' + str(vIJ[0]) + '\tvy:' + str(vIJ[1]) + '\tvz:' + str(vIJ[2]))