本文整理匯總了Python中Quaternion.q_rotMatToQuat方法的典型用法代碼示例。如果您正苦於以下問題：Python Quaternion.q_rotMatToQuat方法的具體用法？Python Quaternion.q_rotMatToQuat怎麽用？Python Quaternion.q_rotMatToQuat使用的例子？那麽, 這裏精選的方法代碼示例或許可以為您提供幫助。您也可以進一步了解該方法所在類Quaternion的用法示例。
在下文中一共展示了Quaternion.q_rotMatToQuat方法的2個代碼示例，這些例子默認根據受歡迎程度排序。您可以為喜歡或者感覺有用的代碼點讚，您的評價將有助於係統推薦出更棒的Python代碼示例。
示例1: test_QuaternionClass
# 需要導入模塊: import Quaternion [as 別名]
# 或者: from Quaternion import q_rotMatToQuat [as 別名]
def test_QuaternionClass(self):
v1 = np.array([0.1, -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 and RotMat ###########################
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 RotMat ###########################
np.testing.assert_almost_equal(Quaternion.q_rotMatToQuat(Quaternion.q_toRotMat(q1)), q1, 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 Exp and RotMat ###########################
np.testing.assert_almost_equal(Quaternion.q_rotVecToRotMat(v1), Quaternion.q_toRotMat(Quaternion.q_exp(v1)), decimal=7)
########################### Testing mult/exp/rotate ###########################
np.testing.assert_almost_equal(Quaternion.q_mult(q1, Quaternion.q_mult(Quaternion.q_exp(v2),Quaternion.q_inverse(q1))),Quaternion.q_exp(Quaternion.q_rotate(q1, v2)), 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)
########################### Testing Jacobian of Exp ###########################
for i in np.arange(0,3):
dv1 = np.array([0.0, 0.0, 0.0])
dv1[i] = 1.0
epsilon = 1e-6
q1 = Quaternion.q_exp(v1)
q1_dist = Quaternion.q_exp(v1+dv1*epsilon)
dq1_1 = Quaternion.q_boxMinus(q1_dist, q1)/epsilon
J = np.resize(Quaternion.q_rotVecToGamma(v1),(3,3))
dq1_2 = J.dot(dv1)
np.testing.assert_almost_equal(dq1_1,dq1_2, decimal=5)示例2:
# 需要導入模塊: import Quaternion [as 別名]
# 或者: from Quaternion import q_rotMatToQuat [as 別名]
timestampTopic = "/okvis/okvis_node/okvis_odometry"
R_BS = np.array(
[
0.0148655429818,
-0.999880929698,
0.00414029679422,
0.999557249008,
0.0149672133247,
0.025715529948,
-0.0257744366974,
0.00375618835797,
0.999660727178,
]
)
bodyTranslation = np.array([-0.0216401454975, -0.064676986768, 0.00981073058949])
bodyRotation = Quaternion.q_inverse(Quaternion.q_rotMatToQuat(R_BS))
inertialTranslation = None
inertialRotation = None
writeData = False
# Load Odometry Data
td.addLabelingIncremental("pos", 3)
td.addLabelingIncremental("att", 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, "pos", "att")
# Load Timestamps into new td
td_aligned.addLabelingIncremental("pos", 3)
td_aligned.addLabelingIncremental("att", 4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic, td_aligned)