当前位置: 首页>>代码示例>>Python>>正文


Python Quaternion.q_toYpr方法代码示例

本文整理汇总了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)
开发者ID:Yvaine,项目名称:trajectory_toolkit,代码行数:35,代码来源:Tests.py

示例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))
开发者ID:ethz-asl,项目名称:trajectory_toolkit,代码行数:6,代码来源:TimedData.py

示例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
开发者ID:ethz-asl,项目名称:trajectory_toolkit,代码行数:79,代码来源:TimedData.py

示例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))
开发者ID:Yvaine,项目名称:trajectory_toolkit,代码行数:4,代码来源:TimedData.py


注:本文中的Quaternion.q_toYpr方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。