这两天卡到一个bug上,很久才发现是因为对数据取精度导致的, 那么按道理来说,精度不应该应该运算的结果,,,实在想不通,有看明白的可以指点一二
main.py
# -*-coding:utf-8 -*-
import numpy as np
from Quaternions import Quaternions
def calculate(refPositions, thisPositions, use_decimals=False):
if use_decimals:
refPositions = np.round(refPositions,decimals=3)
thisPositions = np.round(thisPositions,decimals=3)
o1 = refPositions[0]
a1 = refPositions[1]
b1 = refPositions[2]
o2 = thisPositions[0]
a2 = thisPositions[1]
b2 = thisPositions[2]
oa1 = o1-a1
oa2 = o2-a2
ob1 = o1-b1
ob2 = o2-b2
ab1 = oa1 - ob1
ab2 = oa2 - ob2
q1 = Quaternions.between(ab1,ab2)
aa1 = q1 * oa1
bb1 = q1 * ob1
faabb1 = np.cross(aa1,bb1)
fab2 = np.cross(oa2,ob2)
q2 = Quaternions.between(faabb1,fab2)
q3 = q2 * q1
return q1.qs, q2.qs, q3.qs
if __name__=='__main__':
data = np.load('temp.npy')
# 附件下载地址, 不需要积分: https://download.csdn.net/download/u011771047/75234000
refPositions = data[:3]
thisPositions = data[3:]
_,q21,q31 = calculate(refPositions,thisPositions,use_decimals=False)
_,q22,q32 = calculate(refPositions,thisPositions,use_decimals=True)
print(np.round(q31,decimals=3))
print('---')
print(np.round(q32, decimals=3))
# 这两处输出不一致
>>
[[-0.032 -0.117 0.676 -0.726]]
---
[[ 0. -0. 0.578 -0.816]]
# ------------------------------------------
# 将上面refPositions, thisPositions 的2个数组的内容print到屏幕后,再手动赋值给refPositions1, thisPositions1。 然后再做与上述同样的运算。 得到的两个结果却是一样的. why?
refPositions1 = np.array([[0., 0., 0.],
[-0.5, 0.5, 0.6999999],
[-0.5, -0.50000095, 0.6999999]])
thisPositions1 = np.array([[0., 0., 0.],
[0.50000024, -0.82602787, -0.2401638],
[0.5000001, -0.4950819, 0.70348644]])
_,q23,q33 = calculate(refPositions1,thisPositions1,use_decimals=False)
_,q24,q34 = calculate(refPositions1,thisPositions1,use_decimals=True)
print(np.round(q33,decimals=3))
print('---')
print(np.round(q34, decimals=3))
# 结果一样。
>>
[[ 0. -0. 0.578 -0.816]]
---
[[ 0. -0. 0.578 -0.816]]
Quaternions.py
import numpy as np
class Quaternions:
"""
Quaternions is a wrapper around a numpy ndarray
that allows it to act as if it were an narray of
a quaternion data type.
Therefore addition, subtraction, multiplication,
division, negation, absolute, are all defined
in terms of quaternion operations such as quaternion
multiplication.
This allows for much neater code and many routines
which conceptually do the same thing to be written
in the same way for point data and for rotation data.
The Quaternions class has been desgined such that it
should support broadcasting and slicing in all of the
usual ways.
"""
def __init__(self, qs):
if isinstance(qs, np.ndarray):
if len(qs.shape) == 1: qs = np.array([qs])
self.qs = qs
return
if isinstance(qs, Quaternions):
self.qs = qs.qs
return
raise TypeError('Quaternions must be constructed from iterable, numpy array, or Quaternions, not %s' % type(qs))
@classmethod
def between(cls, v0s, v1s):
a = np.cross(v0s, v1s)
w = np.sqrt((v0s**2).sum(axis=-1) * (v1s**2).sum(axis=-1)) + (v0s * v1s).sum(axis=-1)
return Quaternions(np.concatenate([w[...,np.newaxis], a], axis=-1)).normalized()
其他的小疑惑:
- 四元数q = (w,x, y, z), 当w=0时, 即使x,y,z取任意值,四元数也是表示不同的旋转? 为什么?
q1 = Quaternion(np.array((0., -0.019, -0.302, -0.953)))
q2 = Quaternion(np.array((0., 0., -0.331, -0.944)))