欧几里得变换
欧几里得变换也称为欧式变换、刚性变换,是一种较为基本的变换,通过欧几里得变换,可以改变物体的空间位置,却不改变物体的形状、大小。欧几里得变换包括旋转变换(Rotation transformation)与平移变换(Translation transformation)。至于反射变换(Reflection transformation),是比较有争议的,因为它是通过镜面对称(或平面中的轴对称)或点中心对称变换物体,所以有时认为它不属于欧氏变换,但是有时也认为是欧式变换。在本篇文章,为了将知识点讲述全面,将反射变换归到欧式变换中,并详细讲述旋转、平移与反射变换。
在关于欧式变换的文章中,有些着重讲述欧式变换群——这部分知识属于近世代数的群论部分知识,很少提到具体的操作过程,更多的侧重于抽象的理论。而在本篇文章中,着重讲述变换操作过程与算法实现,推导过程稍作介绍,不作重点,变换群的相关内容放到其它文章中讲解(这一部分与量子力学、量子化学或结构化学等内容相关)。
在变换操作中,一般分为对坐标系的变换以及对坐标系中物体的变换,事实上这两种变换本质上是相同的,如在直角坐标系下物体向某个方向平移一段距离,等价于坐标系向相反方向平移相同距离;物体绕某个轴沿某一方向旋转θ角,等价于坐标轴沿该轴向相反方向旋转θ角。所以,在本篇文章中,下面着重讲述对坐标系中物体的变换。
背景知识
坐标系
关于欧式变换,本文对平移、旋转与反射三种变换分别讲解。每种变换都先从二维平面空间开始,并拓展到三维空间。在每个空间内主要讲解对点的变换操作,因为对线、面、体的变换,与对点的变换有很大的关联(线、面、体由点组成)。并且所有操作都是在平面直角坐标系或空间直角坐标系(右手坐标系)下进行,点的表示分别采用笛卡尔坐标与齐次坐标。
笛卡尔坐标(Cartesian coordinate)
笛卡尔坐标系是我们接触最多的一种坐标系,例如平面直角坐标系、空间直角坐标系等常见的直角坐标系,也包括斜坐标系。在该坐标系下,点可以由一个二维或三维的列向量来表示,如平面中原点 ( 0 , 0 ) T (0,0)^{T} (0,0)T,空间中的某一点 ( a , b , c ) T (a,b,c)^{T} (a,b,c)T等。
齐次坐标(Homogeneous coordinate)
齐次坐标是本文研究变换的一个重点,运用齐次坐标,可以很方便地完成对物体的变换。简单来讲,一个点的齐次坐标就是它的笛卡尔坐标后加上1,如平面中原点的齐次坐标为 ( 0 , 0 , 1 ) T (0,0,1)^T (0,0,1)T,空间中某一点的齐次坐标为 ( a , b , c , 1 ) T (a,b,c,1)^T (a,b,c,1)T。
利用齐次坐标具有诸多优点。例如,判断点是否在直线 a x + b y + c = 0 ax+by+c=0 ax+by+c=0上,只需要判断 ( a , b , c ) ( x , y , 1 ) T (a,b,c)(x,y,1)^T (a,b,c)(x,y,1)T是否为0即可,这时平面直线可以用向量 ( a , b , c ) T (a,b,c)^T (a,b,c)T来表示,空间平面可以用向量 ( a , b , c , d ) T (a,b,c,d)^T (a,b,c,d)T表示,点是否在直线或平面上,只需判断两个向量的内积是否为0即可。
从上面例子中,我们发现,一个直线或平面的方程是不唯一的,上面的直线方程可以写成 k a x + k b y + k c = 0 ( k ! = 0 ) kax+kby+kc=0(k!=0) kax+kby+kc=0(k!=0),也就是说,点的齐次坐标是可以表示成 ( k x , k y , k ) T (kx,ky,k)^T (kx,ky,k)T,即该点有无数种表示方法,如果归一化处理,每个值都除以k,就变成了 ( x , y , 1 ) T (x,y,1)^T (x,y,1)T,也就是前面讲到的齐次坐标的表示方法。如果齐次坐标中最后一个数是0而不是1,该点即表示为无穷远点。
变换的表示方法
在本篇文章中,对于笛卡尔坐标,变换操作即为变换向量/矩阵作用于一个点的笛卡尔坐标(向量),一般为矩阵与向量的相乘或向量之间的相加;对于齐次坐标,即为变换矩阵T作用于一个点的的齐次坐标,运算都是矩阵相乘。本文旋转矩阵用R表示,平移向量由t表示,反射矩阵由S表示,点由向量表示,变换矩阵由T表示。对物体的变换由相应的算子表示,如对点的旋转操作可以表示成 R ^ r \hat{R}r R^r,变换操作表示成 T ^ r \hat{T}r T^r。
点和向量,坐标系公式操作
向量:线性空间中的一个元素,可以把它想象成从原点指向某处的一个箭头。
注意:不要混淆向量和坐标两个概念,只有在指定这个三维空间中的某个坐标系时,才可以谈论该向量在此坐标下的坐标。
如果确定了一个坐标系,也就是一个线性空间的基
(
e
1
,
e
2
,
e
3
)
(e1,e2,e3)
(e1,e2,e3),那么就可以谈论向量a在这组基下的坐标。
坐标的具体取值,一是和向量本身有关,二是和坐标系的选取有关。
对于
a
和
b
∈
R
3
a和b\in R^3
a和b∈R3,内积可以写成:
内积可以描述向量间的投影关系。
外积:
外积的方向垂直于这两个向量,大小为,是两个向量张成的四边形的有向面积。
外积只对三维向量存在定义,我们还能用外积表示向量的旋转。
平移变换
原理
平移变换也是最为简单的一种变换。以平面中点的平移为例:设平移向量
t
=
(
a
,
b
)
T
\mathbf{t}=(a,b)^{T}
t=(a,b)T,一个点的原坐标为
r
=
(
x
,
y
)
T
\textbf{r}=(x,y)^{T}
r=(x,y)T,则平移后的点
r
+
t
\textbf{r}+\boldsymbol{t}
r+t表示为点r延向量t方向平移t的模长距离后的点。对于三维空间的点也是如此。
如果采用齐次坐标
(
x
,
y
,
1
)
T
(x,y,1)^{T}
(x,y,1)T表示r,那么平移变换就可以表示成:
T
^
r
=
T
.
r
=
(
I
2
×
2
t
o
T
1
)
.
r
\hat{T}\mathbf{r}=T.\textbf{r}=\begin{pmatrix} I_{2 \times 2} & t \\ o^T & 1 \end{pmatrix}.r
T^r=T.r=(I2×2oTt1).r
其中 I 2 × 2 I_{2\times 2} I2×2为单位阵, t t t为平移向量, o o o为全0且维数为2的列向量。对于空间平移变换,单位阵为3×3维,向量 t t t与 o o o也都是三维列向量,变换矩阵为4×4矩阵。
import numpy as np
def translation(t,r):
'''三维空间平移操作,t:平移的方向与距离,r:初始点齐次坐标'''
T=np.mat([
[1,0,0,t[0,0]],
[0,1,0,t[1,0]],
[0,0,1,t[2,0]],
[0,0,0,t[3,0]]])
r=np.dot(T,r)
return r
r0=np.mat([0,0,0,1]).T #初始点,齐次向量
t=np.mat([1,2,3,1]).T #平移向量,齐次向量
r1=translation(t,r0) #平移后点的齐次坐标
print(r1)
最终结果的齐次坐标,如果去除最后一个1,即得到笛卡尔坐标。
旋转变换
原理
旋转变换与平移变换相比更为复杂一些,它需要确定旋转轴与旋转角度θ,我们通常认为绕轴逆时针旋转θ为正,逆时针旋转为负。物体在二维平面中的旋转,旋转轴是不能在平面内或与平面斜交(此时旋转物体会脱离平面,就变成了三维空间绕轴旋转),而是垂直于该平面的,所以在平面中看,是物体绕着一个点的旋转。我们先以平面一点绕坐标原点顺时针转θ角(坐标绕原点逆时针转θ)为例,推导过程如下。
上面采用两种方法来推导,第一种是根据向量在各个坐标系下不变,第二种为几何法。对于第一种方法,p表示坐标点在原坐标系下的坐标,p’表示在旋转后坐标系下的坐标,i,j为原坐标系的基矢,i’,j’为旋转后的坐标系的基矢,并且i‘,j’可以表示成i,j分别在i’,j’上的投影。如果是逆时针旋转,把θ取相反符号即可。在实际应用中,我们都以物体逆时针旋转角度为正。
在这里,我们不难发现,旋转矩阵是正交矩阵,即 R T = R − 1 R^T=R^{-1} RT=R−1。对点的旋转操作,为旋转矩阵左乘该点向量,如果旋转坐标轴,为旋转矩阵右乘原坐标系的基矢 ( i . j ) (i.j) (i.j)。对物体的旋转等价于对坐标轴沿相反方向的旋转。
若平面中点p绕任意点o(a,b)旋转,推导过程与上面类似,但是较为复杂。比较好的方法是:把点p先沿
(
−
a
,
−
b
)
T
(-a, -b)^T
(−a,−b)T平移,再绕原点旋转,最后再沿
(
a
,
b
)
T
(a, b)^T
(a,b)T平移,这个过程涉及三步操作,可以由平移、旋转、平移这三个变换矩阵T依次相乘,得到的变换矩阵即为点p绕o点旋转的变换矩阵。
R
=
(
c
o
s
θ
−
s
i
n
θ
s
i
n
θ
c
o
s
θ
)
R=\begin{pmatrix} cos\theta &-sin\theta \\sin\theta&cos\theta \end{pmatrix}
R=(cosθsinθ−sinθcosθ)
表示点p绕原点逆时针旋转θ角操作,若平面中的点r用笛卡尔坐标表示,则R.r表示旋转后得到的点的笛卡尔坐标。
平面中的点用齐次坐标表示,旋转变换可以表示成:
T
^
r
=
T
.
r
=
(
R
2
×
2
o
o
T
1
)
.
r
\hat{T}\textbf{r}=T.\mathbf{r}=\begin{pmatrix}R_{2\times 2}&\textbf{o} \\\textbf{o} ^{T}&1\end{pmatrix}.\textbf{r}
T^r=T.r=(R2×2oTo1).r
其中的R即为前面的旋转矩阵,T为变换矩阵。
在三维空间直角坐标系中,物体绕x轴、y轴与z轴逆时针旋转θ角的旋转矩阵分别为:
R
x
(
θ
x
)
=
(
1
0
0
0
c
o
s
(
θ
x
)
−
s
i
n
(
θ
x
)
0
s
i
n
(
θ
x
)
c
o
s
(
θ
x
)
)
R_{x}(\theta x)=\begin{pmatrix} 1&0&0\\ 0&cos(\theta x)&-sin(\theta x) \\ 0&sin(\theta x)&cos(\theta x)\end{pmatrix}
Rx(θx)=⎝⎛1000cos(θx)sin(θx)0−sin(θx)cos(θx)⎠⎞
R
y
(
θ
y
)
=
(
c
o
s
(
θ
y
)
0
s
i
n
(
θ
y
)
0
1
0
−
s
i
n
(
θ
y
)
0
c
o
s
(
θ
y
)
)
R_{y}(\theta y)=\begin{pmatrix}cos(\theta y) &0&sin(\theta y)\\0&1&0\\ -sin(\theta y)&0&cos(\theta y)\end{pmatrix}
Ry(θy)=⎝⎛cos(θy)0−sin(θy)010sin(θy)0cos(θy)⎠⎞
R
z
(
θ
z
)
=
(
c
o
s
(
θ
z
)
−
s
i
n
(
θ
z
)
0
s
i
n
(
θ
z
)
c
o
s
(
θ
z
)
0
0
0
1
)
R_{z}(\theta z)=\begin{pmatrix}cos(\theta z)&-sin(\theta z)&0\\sin(\theta z)&cos(\theta z)&0\\0&0&1 \end{pmatrix}
Rz(θz)=⎝⎛cos(θz)sin(θz)0−sin(θz)cos(θz)0001⎠⎞
如果是物体绕通过原点的任意轴旋转,可以看成分别绕x、y或z轴若干角度得到的。(相继绕两个轴转θ1、θ2角度,等价于绕垂直于这两个轴的轴旋转2θ1+2θ2角度)。这样计算起来比较麻烦,下面给出物体绕通过原点的任意轴旋转的旋转矩阵公式。
R
v
(
θ
)
=
(
c
o
s
(
θ
)
+
(
1
−
c
o
s
(
θ
)
)
x
2
(
1
−
c
o
s
(
θ
)
)
x
y
−
s
i
n
(
θ
)
z
(
1
−
c
o
s
(
θ
)
)
x
z
+
s
i
n
(
θ
)
y
(
1
−
c
o
s
(
θ
)
)
y
x
+
s
i
n
(
θ
)
z
c
o
s
(
θ
)
+
(
1
−
c
o
s
(
θ
)
)
y
2
(
1
−
c
o
s
(
θ
)
)
y
z
−
s
i
n
(
θ
)
x
(
1
−
c
o
s
(
θ
)
)
z
x
−
s
i
n
(
θ
)
y
(
1
−
c
o
s
(
θ
)
)
z
y
+
s
i
n
(
θ
)
x
c
o
s
(
θ
)
+
(
1
−
c
o
s
(
θ
)
)
z
2
)
R_{v}(\theta)=\begin{pmatrix} cos(\theta)+(1-cos(\theta)) x^{2} &(1-cos(\theta))xy-sin(\theta)z&(1-cos(\theta))xz+sin(\theta)y \\ (1-cos(\theta))yx +sin(\theta)z&cos(\theta)+(1-cos(\theta))y^{2}&(1-cos(\theta))yz-sin(\theta)x\\(1-cos(\theta))zx-sin(\theta)y & (1-cos(\theta))zy+sin(\theta)x & cos(\theta)+(1-cos(\theta))z^{2}\end{pmatrix}
Rv(θ)=⎝⎛cos(θ)+(1−cos(θ))x2(1−cos(θ))yx+sin(θ)z(1−cos(θ))zx−sin(θ)y(1−cos(θ))xy−sin(θ)zcos(θ)+(1−cos(θ))y2(1−cos(θ))zy+sin(θ)x(1−cos(θ))xz+sin(θ)y(1−cos(θ))yz−sin(θ)xcos(θ)+(1−cos(θ))z2⎠⎞
其中v为单位向量,表示轴的方向,θ为旋转角度。
import numpy as np
from numpy import sin,cos,pi
import matplotlib.pyplot as plt
def rotation(theta,r):
theta=theta*pi/180 #角度转弧度制
R=np.mat([[cos(theta),-sin(theta)],
[sin(theta),cos(theta)]])
r=np.dot(R,r)
return np.array(r).flatten()
a=[3,0] #初始点
plt.figure(figsize=(6,6))
for i in range(72):
b=rotation(5,a) #每次旋转5°角
a=b
plt.xlim([-5,5])
plt.ylim([-5,5])
plt.scatter(a[0],a[1])
plt.pause(0.1)
欧拉角
欧拉角是描述方位的一种方法
什么是欧拉角
欧拉角将方位(角位移)分解为绕三个互相垂直轴的旋转。任意三个轴和任意顺序都可以,但最有意义的是使用笛卡尔坐标系并按一定顺序所组成的旋转序列。
最常用的约定是所谓的“heading-pitch-bank”约定。它的基本思想是让物体开始于“标准”方位–就是物体坐标轴和惯性坐标轴对齐。
在标准方位上,让物体作heading、pitch、bank旋转,最后物体到达我们想要描述的方位。
欧拉角表示旋转的利与弊
优点: 欧拉角表示旋转的最大好处就是直观,一眼看去就可以清楚知道绕x,y,z轴的旋转角度。
缺点:
- 旋转结果与执行顺序有关,同样(90,90,90)如果按照x,y,z轴顺序旋转(各旋转90度)和按照z,y,x轴顺序旋转(各旋转90度)的结果是不同的 (可以自己用手机实验下)
- 存在万向节死锁的问题。
四元数
四元数的定义
四元数被定义为表示3D空间旋转的一种方式,它有一个三维向量v和一个标量组成, 如: q = (v,w)四元数的形态有些类似于轴角的表示方式,但与轴角的含义完全不同: p = (v,w) = (sin(α/2)n, cos(α/2)) α为旋转角度,n 为旋转轴的单位向量。
反射变换
反射变换与前两种变换有着本质的区别——对于一个物体,旋转与平移只是改变其空间位置,而反射变换可能会改变物体的手性,例如有机化学中的手性分子,通过反射变换后,前后两个物体就无法重合了。但是对于点,则不会出现这种情况。
反射变换实质上是一种对称操作,并且分为两类:镜面对称(即反映操作)与点中心对称(即反演操作)。关于镜面对称:在二维平面中,是物体关于某一条直线对称;在三维空间中,是物体关于某一平面对称,该变换即把物体关于某平面某直线或空间某平面对称,得到新的物体位置。关于点中心对称,即物体上所有点关于某一中心点对称。
镜面对称(反映操作)
对于平面内的点p,笛卡尔坐标表示为(a,b),它关于x轴对称得到的点为p’(a,-b),那么此时可以表示成:
p
′
=
(
1
0
0
−
1
)
.
p
=
S
(
l
)
.
p
p'=\begin{pmatrix}1&0\\0&-1 \end{pmatrix}.p=S(l).p
p′=(100−1).p=S(l).p
对于平面中的任意对称轴
a
′
x
+
b
′
y
+
c
=
0
a'x+b'y+c=0
a′x+b′y+c=0,关于该轴对称操作的反射矩阵为:
S
(
l
)
=
(
1
−
2
a
2
−
2
a
b
−
2
a
c
−
2
a
b
1
−
2
b
2
−
2
b
c
0
0
1
)
S(l)=\begin{pmatrix}1-2a^{2}&-2ab&-2ac\\-2ab&1-2b^{2}&-2bc\\0&0&1 \end{pmatrix}
S(l)=⎝⎛1−2a2−2ab0−2ab1−2b20−2ac−2bc1⎠⎞
其中a,b,c分别为:
a
′
n
,
b
′
n
,
c
′
n
,
n
=
a
′
2
+
b
′
2
\frac{a'}{n},\frac{b'}{n},\frac{c'}{n},n=\sqrt{a'^2+b'^2}
na′,nb′,nc′,n=a′2+b′2。
同理,对于空间中的平面
a
‘
x
+
b
’
y
+
c
‘
z
+
d
’
=
0
a‘x+b’y+c‘z+d’=0
a‘x+b’y+c‘z+d’=0,关于该平面对称操作的反射矩阵为:
S
(
σ
)
=
(
1
−
2
a
2
−
2
a
b
−
2
a
c
−
2
a
d
−
2
a
b
1
−
2
b
2
−
2
b
c
−
2
b
d
−
2
a
c
−
2
b
c
1
−
2
c
2
−
2
d
c
0
0
0
1
)
S(\sigma)= \begin {pmatrix}1-2a^{2}&-2ab&-2ac&-2ad \\-2ab&1-2b^{2}&-2bc&-2bd\\-2ac&-2bc&1-2c^{2}&-2dc \\ 0&0&0& 1 \end{pmatrix}
S(σ)=⎝⎜⎜⎛1−2a2−2ab−2ac0−2ab1−2b2−2bc0−2ac−2bc1−2c20−2ad−2bd−2dc1⎠⎟⎟⎞
其中a,b,c,d分别为:
a
′
n
,
b
′
n
,
c
′
n
,
d
′
n
,
n
=
a
′
2
+
b
′
2
+
c
′
2
\frac{a'}{n},\frac{b'}{n},\frac{c'}{n},\frac{d'}{n},n=\sqrt{a'^{2}+b'^{2}+c'^{2}}
na′,nb′,nc′,nd′,n=a′2+b′2+c′2利用上述公式,只需要反射矩阵左乘某点的齐次坐标,就可以得到对称操作后的点的齐次坐标。
点中心对称(反演操作)
点中心对称相对较为简单,利用初始点与对称点的中点在对称中心这一结论即可推导。
对于平面中 的一点o(a,b),关于该点中心对称的反射矩阵为:
S
(
o
)
=
(
−
1
0
a
0
−
1
b
0
0
1
)
S(o)=\begin{pmatrix}-1&0&a\\0&-1&b\\0&0&1 \end{pmatrix}
S(o)=⎝⎛−1000−10ab1⎠⎞
对于空间中的一点o(a,b,c),关于该点中心对称的反射矩阵为:
S
(
o
)
=
(
−
1
0
0
2
a
0
−
1
0
2
b
0
0
−
1
2
c
0
0
0
1
)
S(o)=\begin{pmatrix}-1&0&0&2a\\0&-1&0&2b\\0&0&-1&2c\\0&0&0&1 \end{pmatrix}
S(o)=⎝⎜⎜⎛−10000−10000−102a2b2c1⎠⎟⎟⎞
利用上述公式,只需反射矩阵左乘某点的齐次坐标,就可以得到变换后的点的齐次坐标。
组合操作
对于变换操作
T
^
\hat{T}
T^,旋转和平移的组合操作可以写成变换矩阵:
T
=
(
R
t
o
T
1
)
T=\begin{pmatrix} R&\textbf{t}\\\textbf{o}^{T}&1 \end{pmatrix}
T=(RoTt1)
如果是二维空间内的变换,旋转矩阵R为2×2维,平移向量t,全0向量o为2维列向量,T为3×3矩阵;如果是三维空间内的变换,则R为3×3矩阵,平移向量t,全0向量o为3维列向量,T为4×4矩阵。而对于反射变换,是比较特殊的,虽然难以拆分成关于R,t , o,1分块矩阵的形式,但是其结构与前者还是有很多相同的地方。使用变换矩阵时物体坐标需要用齐次坐标。
旋转矩阵公式中给出的是绕通过原点任意轴的情况,如果绕不通过原点的轴旋转,只需先进行平移,平移方向为使得对称轴通过原点的移动方向,即原点到对称轴的垂线的垂足形成的的向量的反方向,再旋转θ角度,最后沿着该向量平移即可。求解平移向量,可以根据对称轴的方向向量(a,b,c)写出与该向量垂直且通过原点的平面σ方程:ax+by+cz=0,把对称轴直线方程用参数方程形式表示,联立求解得到垂足,进而得到平移向量。
对于空间中物体的连续操作,总变换矩阵即为各个变换矩阵的连续相乘,即T1.T2…。对于点的变换,只需要变换矩阵左乘该点的齐次坐标即可。
关于平面曲线、空间曲面的欧式变换,我们可以用参数方程来表示曲线或曲面方程,把参数方程写成齐次坐标形式,变换后得到的参数方程即为变换后的参数方程。
如果T是单位阵,可以看成旋转角为0(即不旋转),平移向量为0,也就是恒等操作,物体与原来位置相同。
import numpy as np
from numpy import sin,cos,pi
class Euclidean_transformation:
def __init__(self,r):
r.append(1)
self.r=np.array(r) #初始点的齐次坐标
def rotation(self,theta,v,p=True):
'''旋转操作
v:旋转轴方向矢量
theta:逆时针旋转角度
p:对称轴是否通过原点,默认通过原点,若不通过原点,p为对称轴所通过的某一点'''
x,y,z=v
n=np.sqrt(x**2+y**2+z**2)
x,y,z=x/n,y/n,z/n #把方向向量转换成单位方向向量
theta=theta*pi/180 #弧度制转成角度制
R=np.mat([[cos(theta)+(1-cos(theta))*x**2,(1-cos(theta))*x*y-sin(theta)*z,(1-cos(theta))*x*z+sin(theta)*y,0],
[(1-cos(theta))*y*x+sin(theta)*z,cos(theta)+(1-cos(theta))*y**2,(1-cos(theta))*y*z-sin(theta)*x,0],
[(1-cos(theta))*z*x-sin(theta)*y,(1-cos(theta))*z*y+sin(theta)*x,cos(theta)+(1-cos(theta))*z**2,0],
[0,0,0,1]])
if p==True:
r = np.dot(R, self.r)
return np.array(r).flatten()[0:3]
else:
a=-(x*p[0]+y*p[1]+z*p[2])/(np.sqrt(x**2+y**2+z**2))
t=np.array([p[0]+x*a,p[1]+y*a,p[2]+z*a]) #原点到对称轴垂线垂足的向量
self.translation(-t) #平移
self.r=np.dot(R,np.mat(self.r).T) #旋转
self.translation(t) #平移
def translation(self,t):
'''平移操作
t:平移矩阵'''
t=np.mat(t).T
R=np.mat([[1,0,0,t[0,0]],
[0,1,0,t[1,0]],
[0,0,1,t[2,0]],
[0,0,0,1]])
r=np.dot(R,self.r)
self.r=np.array(r).flatten()
def mirror(self,*sigma):
'''反射变换中的镜像对称
sigma:平面方程中系数a,b,c,d'''
a,b,c,d=sigma
n=np.sqrt(a**2+b**2+c**2)
a,b,c,d=a/n,b/n,c/n,d/n
R=np.mat([[1-2*a**2,-2*a*b,-2*a*c,-2*a*d],
[-2*a*b,1-2*b**2,-2*b*c,-2*b*d],
[-2*a*c,-2*b*c,1-2*c**2,-2*c*d],
[0,0,0,1]])
r=np.dot(R,self.r)
self.r=np.array(r).flatten()
def central(self,*o):
'''反射变换中的中心对称
o:对称中心坐标'''
a,b,c=o
R=np.mat([[-1,0,0,2*a],
[0,-1,0,2*b],
[0,0,-1,2*c],
[0,0,0,1]])
r=np.dot(R,self.r)
self.r=np.array(r).flatten()
if __name__=='__main__':
b=[0,0,0] #初始点坐标
a=Euclidean_transformation(b)
a.translation(t=[1,1,1]) #平移
a.rotation(theta=180,v=[0,0,1],p=[0,1,0]) #旋转
a.mirror(1,2,3,3) #镜像对称
a.central(0,1,1) #中心对称
print(a.r)
补充
使用齐次坐标,可以将加法运算转化为乘法运算
四元数 、欧拉角与旋转矩阵之间的关系
- 四元数->欧拉角
已知四元数:
q
=
(
q
0
,
q
1
,
q
2
,
q
3
)
q = (q_0, q_1, q_2, q_3)
q=(q0,q1,q2,q3)
欧拉角为:
[
α
β
γ
]
=
[
a
t
a
n
2
(
2
(
q
0
q
1
+
q
2
q
3
)
,
1
−
2
(
q
0
q
0
+
q
1
q
1
)
)
a
r
c
s
i
n
(
2
(
q
0
q
2
−
q
1
q
3
)
)
a
t
a
n
2
(
2
(
q
0
q
3
+
q
1
q
2
)
,
1
−
2
(
q
2
2
+
q
3
3
)
)
]
\left[ \begin{matrix} \alpha \\ \beta \\ \gamma \end{matrix} \right] = \left[ \begin{matrix} atan2(2(q_0q_1+q_2q_3), 1-2(q_0q_0+q_1q_1)) \\ arcsin(2(q_0q_2-q_1q_3)) \\ atan2(2(q_0q_3+q_1q_2), 1-2(q_2^2+q_3^3)) \end{matrix} \right]
⎣⎡αβγ⎦⎤=⎣⎡atan2(2(q0q1+q2q3),1−2(q0q0+q1q1))arcsin(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(q22+q33))⎦⎤
但是当β角度为90度时,四元数反向计算欧拉角时会出现奇点,是无法计算的。因为这时候简化后的四元数是这样的:
[ 0.707 c o s ( α − γ 2 ) 0.707 s i n ( α − γ 2 ) 0.707 c o s ( α − γ 2 ) 0.707 s i n ( α − γ 2 ) ] \left[ \begin{matrix} 0.707cos(\frac{\alpha-\gamma}{2}) \\ 0.707sin(\frac{\alpha-\gamma}{2}) \\ 0.707cos(\frac{\alpha-\gamma}{2}) \\ 0.707sin(\frac{\alpha-\gamma}{2}) \end{matrix} \right] ⎣⎢⎢⎡0.707cos(2α−γ)0.707sin(2α−γ)0.707cos(2α−γ)0.707sin(2α−γ)⎦⎥⎥⎤
所以atan2中后面那一项就变成了0:
1
−
2
(
q
2
2
+
q
3
2
)
=
0
1-2(q_2^2+q_3^2)=0
1−2(q22+q32)=0
这时候我们通常令α=0,然后解出欧拉角的值。
- 欧拉角->四元数
已知欧拉角:
α
\alpha
α、
β
\beta
β、
γ
\gamma
γ
四元数为:
q
=
[
c
o
s
(
γ
2
)
0
0
s
i
n
(
γ
2
)
]
[
c
o
s
(
β
2
)
0
s
i
n
(
β
2
)
0
]
[
c
o
s
(
α
2
)
s
i
n
(
α
2
)
0
0
]
=
[
c
o
s
(
α
2
)
c
o
s
(
β
2
)
c
o
s
(
γ
2
)
+
s
i
n
(
α
2
)
s
i
n
(
β
2
)
s
i
n
(
γ
2
)
s
i
n
(
α
2
)
c
o
s
(
β
2
)
c
o
s
(
γ
2
)
−
c
o
s
(
α
2
)
s
i
n
(
β
2
)
s
i
n
(
γ
2
)
c
o
s
(
α
2
)
s
i
n
(
β
2
)
c
o
s
(
γ
2
)
+
s
i
n
(
α
2
)
c
o
s
(
β
2
)
s
i
n
(
γ
2
)
c
o
s
(
α
2
)
c
o
s
(
β
2
)
s
i
n
(
γ
2
)
+
s
i
n
(
α
2
)
s
i
n
(
β
2
)
c
o
s
(
γ
2
)
]
q = \left[ \begin{matrix} cos(\frac{\gamma}{2}) \\ 0 \\ 0 \\ sin(\frac{\gamma}{2}) \end{matrix} \right] \left[ \begin{matrix} cos(\frac{\beta}{2}) \\ 0 \\ sin(\frac{\beta}{2}) \\ 0 \end{matrix} \right] \left[ \begin{matrix} cos(\frac{\alpha}{2}) \\ sin(\frac{\alpha}{2}) \\ 0 \\ 0 \end{matrix} \right]= \left[ \begin{matrix} cos(\frac{\alpha}{2})cos(\frac{\beta}{2})cos(\frac{\gamma}{2}) + sin(\frac{\alpha}{2})sin(\frac{\beta}{2})sin(\frac{\gamma}{2}) \\ sin(\frac{\alpha}{2})cos(\frac{\beta}{2})cos(\frac{\gamma}{2}) - cos(\frac{\alpha}{2})sin(\frac{\beta}{2})sin(\frac{\gamma}{2}) \\ cos(\frac{\alpha}{2})sin(\frac{\beta}{2})cos(\frac{\gamma}{2}) + sin(\frac{\alpha}{2})cos(\frac{\beta}{2})sin(\frac{\gamma}{2}) \\ cos(\frac{\alpha}{2})cos(\frac{\beta}{2})sin(\frac{\gamma}{2}) + sin(\frac{\alpha}{2})sin(\frac{\beta}{2})cos(\frac{\gamma}{2}) \end{matrix} \right]
q=⎣⎢⎢⎡cos(2γ)00sin(2γ)⎦⎥⎥⎤⎣⎢⎢⎡cos(2β)0sin(2β)0⎦⎥⎥⎤⎣⎢⎢⎡cos(2α)sin(2α)00⎦⎥⎥⎤=⎣⎢⎢⎡cos(2α)cos(2β)cos(2γ)+sin(2α)sin(2β)sin(2γ)sin(2α)cos(2β)cos(2γ)−cos(2α)sin(2β)sin(2γ)cos(2α)sin(2β)cos(2γ)+sin(2α)cos(2β)sin(2γ)cos(2α)cos(2β)sin(2γ)+sin(2α)sin(2β)cos(2γ)⎦⎥⎥⎤
- 欧拉角->旋转矩阵
已知欧拉角:
α
\alpha
α、
β
\beta
β、
γ
\gamma
γ
则求解旋转矩阵
R
=
R
z
(
γ
)
R
x
(
α
)
R
y
(
β
)
=
[
c
o
s
(
θ
)
c
o
s
(
γ
)
s
i
n
(
α
)
s
i
n
(
θ
)
c
o
s
(
γ
)
−
c
o
s
(
α
)
s
i
n
(
γ
)
c
o
s
(
α
)
s
i
n
(
θ
)
c
o
s
(
γ
)
+
s
i
n
(
α
)
s
i
n
(
γ
)
c
o
s
(
θ
)
s
i
n
(
γ
)
s
i
n
(
α
)
s
i
n
(
θ
)
s
i
n
(
γ
)
+
c
o
s
(
θ
)
c
o
s
(
γ
)
c
o
s
(
α
)
s
i
n
(
θ
)
s
i
n
(
γ
)
−
s
i
n
(
α
)
c
o
s
(
γ
)
−
s
i
n
(
θ
)
s
i
n
(
α
)
c
o
s
(
θ
)
c
o
s
(
α
)
c
o
s
(
θ
)
]
R=R_z(\gamma)R_x(\alpha)R_y(\beta)=\left[ \begin{matrix} cos(\theta)cos(\gamma) & sin(\alpha)sin(\theta)cos(\gamma)-cos(\alpha)sin(\gamma) & cos(\alpha)sin(\theta)cos(\gamma)+sin(\alpha)sin(\gamma) \\ cos(\theta)sin(\gamma) & sin(\alpha)sin(\theta)sin(\gamma)+cos(\theta)cos(\gamma) & cos(\alpha)sin(\theta)sin(\gamma)-sin(\alpha)cos(\gamma) \\ -sin(\theta) & sin(\alpha)cos(\theta) & cos(\alpha)cos(\theta) \end{matrix} \right]
R=Rz(γ)Rx(α)Ry(β)=⎣⎡cos(θ)cos(γ)cos(θ)sin(γ)−sin(θ)sin(α)sin(θ)cos(γ)−cos(α)sin(γ)sin(α)sin(θ)sin(γ)+cos(θ)cos(γ)sin(α)cos(θ)cos(α)sin(θ)cos(γ)+sin(α)sin(γ)cos(α)sin(θ)sin(γ)−sin(α)cos(γ)cos(α)cos(θ)⎦⎤
- 旋转矩阵->欧拉角
已知旋转矩阵:
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
R=\left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix} \right]
R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
则欧拉角为
{
α
=
a
t
a
n
2
(
r
32
,
r
33
)
θ
=
a
t
a
n
2
(
−
r
31
,
r
32
2
+
r
33
2
)
γ
=
a
t
a
n
2
(
r
21
,
r
11
)
\left\{\begin{aligned} \alpha = atan2(r_{32,r_{33}})\\ \theta = atan2(-r_{31}, \sqrt{r^2_{32}+r^2_{33}})\\ \gamma = atan2(r_{21}, r_{11}) \end{aligned} \right.
⎩⎪⎪⎨⎪⎪⎧α=atan2(r32,r33)θ=atan2(−r31,r322+r332)γ=atan2(r21,r11)
- 旋转矩阵->四元数
已知旋转矩阵:
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
R=\left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix} \right]
R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
则求解四元数时根据的方法就是从四元数转旋转矩阵的公式中得到:
r
11
+
r
22
+
r
33
=
2
q
0
2
−
1
⇒
q
0
=
±
1
2
1
+
r
11
+
r
22
+
r
33
r_{11}+r_{22}+r_{33}=2q_0^{2}-1 \Rightarrow q_0 = \pm \frac{1}{2} \sqrt{1+r_{11}+r_{22}+r_{33}}
r11+r22+r33=2q02−1⇒q0=±211+r11+r22+r33
r
11
−
r
22
−
r
33
=
2
q
1
2
−
1
⇒
q
1
=
±
1
2
1
+
r
11
−
r
22
−
r
33
r_{11}-r_{22}-r_{33}=2q_1^{2}-1 \Rightarrow q_1 = \pm \frac{1}{2} \sqrt{1+r_{11}-r_{22}-r_{33}}
r11−r22−r33=2q12−1⇒q1=±211+r11−r22−r33
r
22
−
r
11
−
r
33
=
2
q
2
2
−
1
⇒
q
2
=
±
1
2
1
+
r
22
−
r
11
−
r
33
r_{22}-r_{11}-r_{33}=2q_2^{2}-1 \Rightarrow q_2 = \pm \frac{1}{2} \sqrt{1+r_{22}-r_{11}-r_{33}}
r22−r11−r33=2q22−1⇒q2=±211+r22−r11−r33
r
33
−
r
11
+
r
22
=
2
q
3
2
−
1
⇒
q
3
=
±
1
2
1
+
r
33
−
r
11
−
r
22
r_{33}-r_{11}+r_{22}=2q_3^{2}-1 \Rightarrow q_3 = \pm \frac{1}{2} \sqrt{1+r_{33}-r_{11}-r_{22}}
r33−r11+r22=2q32−1⇒q3=±211+r33−r11−r22
但从上式中是无法确定正负号的,所以又有:
r
12
+
r
21
=
2
(
q
1
q
2
−
q
0
q
3
)
+
2
(
q
1
q
2
+
q
0
q
3
)
=
4
q
1
q
2
r_{12}+r_{21}=2(q_1q_2-q_0q_3)+2(q_1q_2+q_0q_3)=4q_1q_2
r12+r21=2(q1q2−q0q3)+2(q1q2+q0q3)=4q1q2
r
21
−
r
12
=
2
(
q
1
q
2
+
q
0
q
3
)
−
2
(
q
1
q
2
−
q
0
q
3
)
=
4
q
0
q
3
r_{21}-r_{12}=2(q_1q_2+q_0q_3)-2(q_1q_2-q_0q_3)=4q_0q_3
r21−r12=2(q1q2+q0q3)−2(q1q2−q0q3)=4q0q3
r
13
+
r
31
=
2
(
q
1
q
3
+
q
0
q
2
)
+
2
(
q
1
q
3
−
q
0
q
2
)
=
4
q
1
q
3
r_{13}+r_{31}=2(q_1q_3+q_0q_2)+2(q_1q_3-q_0q_2)=4q_1q_3
r13+r31=2(q1q3+q0q2)+2(q1q3−q0q2)=4q1q3
r
31
−
r
13
=
2
(
q
1
q
3
+
q
0
q
2
)
−
2
(
q
1
q
3
−
q
0
q
2
)
=
4
q
0
q
2
r_{31}-r_{13}=2(q_1q_3+q_0q_2)-2(q_1q_3-q_0q_2)=4q_0q_2
r31−r13=2(q1q3+q0q2)−2(q1q3−q0q2)=4q0q2
r
23
+
r
32
=
2
(
q
2
q
3
−
q
0
q
1
)
+
2
(
q
2
q
3
+
q
0
q
1
)
=
4
q
2
q
3
r_{23}+r_{32}=2(q_2q_3-q_0q_1)+2(q_2q_3+q_0q_1)=4q_2q_3
r23+r32=2(q2q3−q0q1)+2(q2q3+q0q1)=4q2q3
r
32
−
r
23
=
2
(
q
2
q
3
+
q
0
q
1
)
−
2
(
q
2
q
3
−
q
0
q
1
)
=
4
q
0
q
1
r_{32}-r_{23}=2(q_2q_3+q_0q_1)-2(q_2q_3-q_0q_1)=4q_0q_1
r32−r23=2(q2q3+q0q1)−2(q2q3−q0q1)=4q0q1
这样只要得到q0到q4中的任意一个就能根据上面的关系求出剩余3个分量的值,假设我们先求q0的值,则有:
{
q
0
=
±
1
2
1
+
r
11
+
r
22
+
r
33
q
1
=
r
32
−
r
23
4
q
0
q
2
=
r
13
−
r
31
4
q
0
q
3
=
r
21
−
r
12
4
q
0
\left\{\begin{aligned} q_0 = \pm \frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} \\ q_1 = \frac{r_{32} - r_{23}}{4q_0} \\ q_2 = \frac{r_{13} - r_{31}}{4q_0} \\ q_3 = \frac{r_{21} - r_{12}}{4q_0} \end{aligned} \right.
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧q0=±211+r11+r22+r33q1=4q0r32−r23q2=4q0r13−r31q3=4q0r21−r12
从上式中可以看到,求得的四元数有两个,但他们表示的是同一种旋转关系,至于先求q0到q4中的哪个值,在实际使用时应该全部一起求,看哪个值大,就选取哪个,以防止某一项在出现0时无法计算的情况。
- 四元数->旋转矩阵
已知四元数:
q
=
(
q
0
,
q
1
,
q
2
,
q
3
)
q = (q_0, q_1, q_2, q_3)
q=(q0,q1,q2,q3)
旋转矩阵为:
R
b
n
=
[
q
0
2
+
q
1
2
−
q
2
2
−
q
3
2
2
(
q
1
q
2
−
q
0
q
3
)
2
(
q
1
q
3
+
q
0
q
2
)
2
(
q
1
q
2
+
q
0
q
3
)
q
0
2
−
q
1
2
+
q
2
2
−
q
3
2
2
(
q
2
q
3
−
q
0
q
1
)
2
(
q
1
q
3
−
q
0
q
2
)
2
(
q
2
q
3
+
q
0
q
1
)
q
0
2
−
q
1
2
−
q
2
2
+
q
3
2
]
R_b^n=\left[ \begin{matrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2+q_0q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3-q_0q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{matrix} \right]
Rbn=⎣⎡q02+q12−q22−q322(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)q02−q12+q22−q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)q02−q12−q22+q32⎦⎤
参考
https://www.cnblogs.com/yfqh/p/11025470.html
https://blog.csdn.net/weixin_60737527/article/details/125594852
https://blog.csdn.net/weixin_60737527/article/details/125013910