本文整理汇总了Python中Quaternion.q_toYpr方法的典型用法代码示例。如果您正苦于以下问题：Python Quaternion.q_toYpr方法的具体用法？Python Quaternion.q_toYpr怎么用？Python Quaternion.q_toYpr使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Quaternion的用法示例。
在下文中一共展示了Quaternion.q_toYpr方法的4个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: test_QuaternionClass
# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_toYpr [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)示例2: quaternionToYpr
# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_toYpr [as 别名]
def quaternionToYpr(self, att, ypr):
attIDs = self.getColIDs(att)
yprIDs = self.getColIDs(ypr)
self.d[0:self.end(),yprIDs] = Quaternion.q_toYpr(self.col(attIDs))示例3: computeLeutiScore
# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_toYpr [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示例4: quaternionToYpr
# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_toYpr [as 别名]
def quaternionToYpr(self, attIDs, yprIDs):
self.d[0:self.end(),yprIDs] = Quaternion.q_toYpr(self.cols(attIDs))