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


Python Quaternion.q_Lmat方法代码示例

本文整理汇总了Python中Quaternion.q_Lmat方法的典型用法代码示例。如果您正苦于以下问题:Python Quaternion.q_Lmat方法的具体用法?Python Quaternion.q_Lmat怎么用?Python Quaternion.q_Lmat使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在Quaternion的用法示例。


在下文中一共展示了Quaternion.q_Lmat方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。

示例1: test_QuaternionClass

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

# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_Lmat [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
开发者ID:ethz-asl,项目名称:trajectory_toolkit,代码行数:42,代码来源:TimedData.py


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