本文整理汇总了Python中Quaternion.q_log方法的典型用法代码示例。如果您正苦于以下问题：Python Quaternion.q_log方法的具体用法？Python Quaternion.q_log怎么用？Python Quaternion.q_log使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Quaternion的用法示例。
在下文中一共展示了Quaternion.q_log方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: test_QuaternionClass
# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_log [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: ror
# 需要导入模块: import Quaternion [as 别名]
# 或者: from Quaternion import q_log [as 别名]
# Calculate the Norm of the Rotational Rate provide ror(td1=[14,15,16], td2=[5,6,7]) and rorNorm(td1=17,td2=8) column IDs.
td1.computeNormOfColumns(rorIDs1,rorNID1)
td2.computeNormOfColumns(rorIDs2,rorNID2)
"""
We can estimate the time offset using the norm of the rotational rate.
The estimated time offset is then applied to td2.
"""
to = td2.getTimeOffset(rorNID2,td1,rorNID1)
td2.applyTimeOffset(-to)
"""
The calibration of the Body Transform needs the velocity and the rotational rate start IDs.
"""
B_r_BC_est, qCB_est = td1.calibrateBodyTransform(velBIDs1,rorIDs1,td2, velBIDs2,rorIDs2)
vCB_est = Quaternion.q_log(qCB_est)
vCB_err = vCB-vCB_est
B_r_BC_err = B_r_BC - B_r_BC_est
print('Calibrate Body Transform:')
print('Rotation Vector ln(qCB_est):\tvx:' + str(vCB_est[0]) + '\tvy:' + str(vCB_est[1]) + '\tvz:' + str(vCB_est[2]))
print('Translation Vector B_r_BC_est:\trx:' + str(B_r_BC_est[0]) + '\try:' + str(B_r_BC_est[1]) + '\trz:' + str(B_r_BC_est[2]))
print('Rotation Error ln(qCB_err):\tvx:' + str(vCB_err[0]) + '\tvy:' + str(vCB_err[1]) + '\tvz:' + str(vCB_err[2]))
print('Translation Error B_r_BC_err:\trx:' + str(B_r_BC_err[0]) + '\try:' + str(B_r_BC_err[1]) + '\trz:' + str(B_r_BC_err[2]))
"""
The calibration of the Intertial Transform needs the velocity and the rotational rate start IDs and the estimated body transform.
"""
J_r_JI_est, qIJ_est = td1.calibrateInertialTransform(posIDs1, attIDs1, td2, posIDs2, attIDs2, B_r_BC_est, qCB_est, [0,1,2])
vIJ_est = Quaternion.q_log(qIJ_est);
vIJ_err = vIJ-vIJ_est;
J_r_JI_err = J_r_JI - J_r_JI_est;