版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/u011178262/article/details/84479181
理论基础
三维空间中的变换主要分为如下几种:
其性质如下图所示:
本文主要介绍欧式变换。
欧式变换
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4}
T = [ R 0 T t 1 ] ∈ R 4 × 4
T
−
1
=
[
R
T
−
R
T
⋅
t
0
T
1
]
∈
R
4
×
4
\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T \cdot \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4}
T − 1 = [ R T 0 T − R T ⋅ t 1 ] ∈ R 4 × 4
Translate by
−
C
-C
− C (align origins), Rotate to align axes:
P
c
=
T
⋅
P
w
=
R
⋅
(
P
w
−
C
)
=
R
⋅
P
w
−
R
⋅
C
=
R
⋅
P
w
+
t
\begin{aligned} P_c &= \mathbf{T} \cdot P_w \\ &= \mathbf{R} \cdot (P_w - C) \\ &= \mathbf{R} \cdot P_w - \mathbf{R} \cdot C \\ &= \mathbf{R} \cdot P_w + \mathbf{t} \end{aligned}
P c = T ⋅ P w = R ⋅ ( P w − C ) = R ⋅ P w − R ⋅ C = R ⋅ P w + t
旋转
旋转矩阵
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
∈
R
3
×
3
,
s
.
t
.
R
R
T
=
I
,
d
e
t
(
R
)
=
1
\mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \in \mathbb{R}^{3 \times 3}, \quad s.t. \quad \mathbf{RR}^T = \mathbf{I}, det(\mathbf{R}) = 1
R = ⎣ ⎡ r 1 1 r 2 1 r 3 1 r 1 2 r 2 2 r 3 2 r 1 3 r 2 3 r 3 3 ⎦ ⎤ ∈ R 3 × 3 , s . t . R R T = I , d e t ( R ) = 1
旋转向量(轴角)
ξ
=
α
a
=
l
o
g
(
R
)
∨
∈
R
3
\boldsymbol{\xi} = \alpha\mathbf{a} = log(\mathbf{R})^{\vee} \in \mathbb{R}^3
ξ = α a = l o g ( R ) ∨ ∈ R 3
旋转轴 :矩阵
R
\mathbf{R}
R 特征值1对应的特征向量(单位矢量)
a
=
ξ
∣
∣
ξ
∣
∣
∈
R
3
\mathbf{a} = \frac{\boldsymbol{\xi}}{||\boldsymbol{\xi}||} \in \mathbb{R}^3
a = ∣ ∣ ξ ∣ ∣ ξ ∈ R 3
旋转角
α
=
∣
∣
ξ
∣
∣
=
a
r
c
c
o
s
(
t
r
(
R
)
−
1
2
)
∈
R
\alpha = ||\boldsymbol{\xi}|| = arccos(\frac{tr(\mathbf{R})-1}{2}) \in \mathbb{R}
α = ∣ ∣ ξ ∣ ∣ = a r c c o s ( 2 t r ( R ) − 1 ) ∈ R
罗德里格斯公式(Rodrigues’ rotation formula ):
R
=
c
o
s
α
I
+
(
1
−
c
o
s
α
)
a
a
T
+
s
i
n
α
a
∧
\mathbf{R} = cos\alpha \mathbf{I} + (1-cos\alpha) \mathbf{aa}^T + sin\alpha \mathbf{a}^{\wedge}
R = c o s α I + ( 1 − c o s α ) a a T + s i n α a ∧
单位四元数
2D旋转 :单位复数 可用来表示2D旋转。
z
=
a
+
b
i
⃗
=
r
(
c
o
s
θ
+
s
i
n
θ
i
⃗
)
=
e
θ
i
⃗
,
r
=
∣
∣
z
∣
∣
=
1
z = a + b\vec{i} = r ( cos\theta + sin\theta\vec{i} ) = e^{\theta \vec{i}}, r = ||z||=1
z = a + b i
= r ( c o s θ + s i n θ i
) = e θ i
, r = ∣ ∣ z ∣ ∣ = 1
3D旋转 :单位四元数 才可表示3D旋转,四元数是复数的扩充,在表示旋转前需要进行 归一化 。
q
=
[
ε
η
]
s
.
t
.
∣
∣
q
∣
∣
2
=
1
\mathbf{q} = \begin{bmatrix} \boldsymbol\varepsilon \\ \eta \end{bmatrix} \quad s.t. \quad ||\mathbf{q}||_2 = 1
q = [ ε η ] s . t . ∣ ∣ q ∣ ∣ 2 = 1
where
η
=
c
o
s
α
2
,
ε
=
a
s
i
n
α
2
=
[
a
1
s
i
n
α
2
a
2
s
i
n
α
2
a
3
s
i
n
α
2
]
=
[
ε
1
ε
2
ε
3
]
\eta = cos\frac{\alpha}{2}, \quad \boldsymbol\varepsilon = \mathbf{a} sin\frac{\alpha}{2} = \begin{bmatrix} a_1sin \frac{\alpha}{2} \\ a_2sin \frac{\alpha}{2} \\ a_3sin \frac{\alpha}{2} \end{bmatrix} = \begin{bmatrix} \varepsilon_1 \\ \varepsilon_2 \\ \varepsilon_3 \end{bmatrix}
η = c o s 2 α , ε = a s i n 2 α = ⎣ ⎡ a 1 s i n 2 α a 2 s i n 2 α a 3 s i n 2 α ⎦ ⎤ = ⎣ ⎡ ε 1 ε 2 ε 3 ⎦ ⎤
and
∣
∣
a
∣
∣
2
=
1
,
η
2
+
ε
1
2
+
ε
2
2
+
ε
3
2
=
1
||\mathbf{a}||_2 = 1, \quad \eta^2 + \varepsilon_1^2 + \varepsilon_2^2 + \varepsilon_3^2 = 1
∣ ∣ a ∣ ∣ 2 = 1 , η 2 + ε 1 2 + ε 2 2 + ε 3 2 = 1
四元数可以在 保证效率 的同时,减小矩阵1/4的内存占有量,同时又能 避免欧拉角的万向锁问题 。
欧拉角
旋转矩阵可以可以分解为绕各自轴对应旋转矩阵的乘积:
R
=
R
1
R
2
R
3
\mathbf{R} = \mathbf{R}_1 \mathbf{R}_2 \mathbf{R}_3
R = R 1 R 2 R 3
根据绕轴的不同,欧拉角共分为两大类,共12种,如下图(基于 右手系 )所示: 以上不同旋转轴合成的旋转矩阵,每一种都可以看成 同一旋转矩阵的两种不同物理变换 :
以
Z
1
Y
2
X
3
Z_1Y_2X_3
Z 1 Y 2 X 3 进行为例,旋转矩阵表示为
R
=
R
z
R
y
R
x
\mathbf{R} = \mathbf{R}_z \mathbf{R}_y \mathbf{R}_x
R = R z R y R x ,说明:
绕 固定轴 旋转:以初始坐标系作为固定坐标系,分别先后绕固定坐标系的X、Y、Z轴 旋转;
绕 动轴 旋转:先绕 初始Z轴 旋转,再绕 变换后的Y轴 旋转,最后绕 变换后的X轴 旋转
即 绕 固定坐标轴的XYZ 和 绕运动坐标轴的ZYX 的旋转矩阵是一样的。
我们经常用的欧拉角一般就是
Z
1
Y
2
X
3
Z_1Y_2X_3
Z 1 Y 2 X 3 轴序的 yaw-pitch-roll ,如下图所示:
对应的旋转矩阵为
R
=
R
z
R
y
R
x
=
R
(
θ
y
a
w
)
R
(
θ
p
i
t
c
h
)
R
(
θ
r
o
l
l
)
\mathbf{R} = \mathbf{R}_z \mathbf{R}_y \mathbf{R}_x = \mathbf{R}(\theta_{yaw}) \mathbf{R}(\theta_{pitch}) \mathbf{R}(\theta_{roll})
R = R z R y R x = R ( θ y a w ) R ( θ p i t c h ) R ( θ r o l l )
其逆矩阵为:
R
−
1
=
(
R
z
R
y
R
x
)
−
1
=
R
x
−
1
R
y
−
1
R
z
−
1
=
R
(
−
θ
r
o
l
l
)
R
(
−
θ
p
i
t
c
h
)
R
(
−
θ
y
a
w
)
\begin{aligned} \mathbf{R}^{-1} &= (\mathbf{R}_z \mathbf{R}_y \mathbf{R}_x)^{-1} \\ &= \mathbf{R}_x^{-1} \mathbf{R}_y^{-1} \mathbf{R}_z^{-1} \\ &= \mathbf{R}(-\theta_{roll}) \mathbf{R}(-\theta_{pitch}) \mathbf{R}(-\theta_{yaw}) \end{aligned}
R − 1 = ( R z R y R x ) − 1 = R x − 1 R y − 1 R z − 1 = R ( − θ r o l l ) R ( − θ p i t c h ) R ( − θ y a w )
上面
R
x
R
y
R
z
\mathbf{R}_x \mathbf{R}_y \mathbf{R}_z
R x R y R z 以 Cosine Matrix 的形式表示为(右手系 ):
R
x
(
θ
)
=
[
1
0
0
0
c
o
s
(
θ
)
−
s
i
n
(
θ
)
0
s
i
n
(
θ
)
c
o
s
(
θ
)
]
\mathbf{R}_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{bmatrix}
R x ( θ ) = ⎣ ⎡ 1 0 0 0 c o s ( θ ) s i n ( θ ) 0 − s i n ( θ ) c o s ( θ ) ⎦ ⎤
R
y
(
θ
)
=
[
c
o
s
(
θ
)
0
s
i
n
(
θ
)
0
1
0
−
s
i
n
(
θ
)
0
c
o
s
(
θ
)
]
\mathbf{R}_y(\theta) = \begin{bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \end{bmatrix}
R y ( θ ) = ⎣ ⎡ c o s ( θ ) 0 − s i n ( θ ) 0 1 0 s i n ( θ ) 0 c o s ( θ ) ⎦ ⎤
R
z
(
θ
)
=
[
c
o
s
(
θ
)
−
s
i
n
(
θ
)
0
s
i
n
(
θ
)
c
o
s
(
θ
)
0
0
0
1
]
\mathbf{R}_z(\theta) = \begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix}
R z ( θ ) = ⎣ ⎡ c o s ( θ ) s i n ( θ ) 0 − s i n ( θ ) c o s ( θ ) 0 0 0 1 ⎦ ⎤
旋转转换
平移
t
=
[
x
y
z
]
T
∈
R
3
\mathbf{t} = \begin{bmatrix} x & y & z \end{bmatrix}^T \in \mathbb{R}^3
t = [ x y z ] T ∈ R 3
李群和李代数
特殊正交群
S
O
(
3
)
SO(3)
S O ( 3 )
S
O
(
3
)
=
{
R
∈
R
3
×
3
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(3) = \Bigg\{ \mathbf{R} \in \mathbb{R}^{3 \times 3} \Bigg| \mathbf{RR}^T = \mathbf{I}, det(\mathbf{R}) = 1 \Bigg\}
S O ( 3 ) = { R ∈ R 3 × 3 ∣ ∣ ∣ ∣ ∣ R R T = I , d e t ( R ) = 1 }
李代数
s
o
(
3
)
\mathfrak{so}(3)
s o ( 3 )
s
o
(
3
)
=
{
Φ
=
ϕ
∧
∈
R
3
×
3
∣
ϕ
∈
R
3
}
\mathfrak{so}(3) = \Bigg\{ \boldsymbol{\Phi} = \boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3} \Bigg| \boldsymbol{\phi} \in \mathbb{R}^3 \Bigg\}
s o ( 3 ) = { Φ = ϕ ∧ ∈ R 3 × 3 ∣ ∣ ∣ ∣ ∣ ϕ ∈ R 3 }
where
ϕ
∧
=
[
ϕ
1
ϕ
2
ϕ
3
]
∧
=
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
∈
R
3
×
3
\boldsymbol{\phi}^{\wedge} = \begin{bmatrix} \phi_1 \\ \phi_2 \\ \phi_3 \end{bmatrix}^{\wedge} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 3}
ϕ ∧ = ⎣ ⎡ ϕ 1 ϕ 2 ϕ 3 ⎦ ⎤ ∧ = ⎣ ⎡ 0 ϕ 3 − ϕ 2 − ϕ 3 0 ϕ 1 ϕ 2 − ϕ 1 0 ⎦ ⎤ ∈ R 3 × 3
指数映射:
R
=
e
x
p
(
ϕ
∧
)
≈
I
+
ϕ
∧
\mathbf{R} = exp(\boldsymbol{\phi}^{\wedge}) {\approx} \mathbf{I} + \boldsymbol{\phi}^{\wedge}
R = e x p ( ϕ ∧ ) ≈ I + ϕ ∧ (first-order approximation)
对数映射:
ϕ
=
l
o
g
(
R
)
∨
\boldsymbol{\phi} = log(\mathbf{R})^{\vee}
ϕ = l o g ( R ) ∨
特殊欧式群
S
E
(
3
)
SE(3)
S E ( 3 )
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3) = \Bigg\{ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \Bigg| \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^{3} \Bigg\}
S E ( 3 ) = { T = [ R 0 T t 1 ] ∈ R 4 × 4 ∣ ∣ ∣ ∣ ∣ R ∈ S O ( 3 ) , t ∈ R 3 }
李代数
s
e
(
3
)
\mathfrak{se}(3)
s e ( 3 )
s
e
(
3
)
=
{
Ξ
=
ξ
∧
∈
R
4
×
4
∣
ξ
∈
R
6
}
\mathfrak{se}(3) = \Bigg\{ \boldsymbol{\Xi} = \boldsymbol{\xi}^{\wedge} \in \mathbb{R}^{4 \times 4} \Bigg| \boldsymbol{\xi} \in \mathbb{R}^6 \Bigg\}
s e ( 3 ) = { Ξ = ξ ∧ ∈ R 4 × 4 ∣ ∣ ∣ ∣ ∣ ξ ∈ R 6 }
where
ξ
∧
=
[
ρ
ϕ
]
∧
=
[
ϕ
∧
ρ
0
T
,
0
]
∈
R
4
×
4
,
ρ
,
ϕ
∈
R
3
\boldsymbol{\xi}^{\wedge} = \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{bmatrix}^{\wedge} = \begin{bmatrix} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T, & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4}, \quad \boldsymbol{\rho},\boldsymbol{\phi} \in \mathbb{R}^3
ξ ∧ = [ ρ ϕ ] ∧ = [ ϕ ∧ 0 T , ρ 0 ] ∈ R 4 × 4 , ρ , ϕ ∈ R 3
指数映射:
T
=
e
x
p
(
ξ
∧
)
\mathbf{T} = exp(\boldsymbol{\xi}^{\wedge})
T = e x p ( ξ ∧ ) 对数映射:
ξ
=
l
o
g
(
T
)
∨
\boldsymbol{\xi} = log(\mathbf{T})^{\vee}
ξ = l o g ( T ) ∨
坐标系的手性主要分为 右手系 和 左手系 ,主要通过以下两种方法区分(右手系):
3 finger method
Curling method
另外,不同的几何编程库所基于的坐标系的手性会有所不同
Eigen: 右手系
OpenGL: 右手系
Unity3D: 左手系
ROS tf: 右手系
注意事项
区分 点的变换 和 坐标系本身的变换
P
a
=
T
A
B
⋅
P
b
P_a = \mathbf{T}_{AB} \cdot P_b
P a = T A B ⋅ P b
指的是 将某点在B坐标系中的坐标表示变换为其在A坐标系中的坐标表示,实质是同一点在不同坐标系下的不同坐标表示,即 点的变换 ;若将A和B坐标系假设为刚体,则B坐标系变换到A坐标系(坐标系本身的变换 )的变换矩阵为
T
A
B
−
1
\mathbf{T}_{AB}^{-1}
T A B − 1 。
在分析多个坐标系的姿态变换时,要注意根据点的变换或者坐标系的变换确定矩阵左乘还是右乘:
点的变换 :矩阵相乘 从右到左,即 矩阵左乘
坐标系的变换 :矩阵相乘 从左到右,即 矩阵右乘
区分 绕定轴旋转 和 绕动轴旋转
注意 右手系 和 左手系
注意 同一刚体中不同坐标系姿态变换的相互表示
以带有IMU的相机模组为例,已知 IMU(坐标系)本身的姿态变换
T
B
\mathbf{T}^{B}
T B 和 同一模组中Camera到IMU(Body)的坐标系变换
T
B
C
\mathbf{T}_{BC}
T B C ,则 该Camera(坐标系)本身的姿态变换为:
C
T
=
T
B
C
⋅
T
B
⋅
T
B
C
−
1
{}_C\mathbf{T} = \mathbf{T}_{BC} \cdot \mathbf{T}^{B} \cdot \mathbf{T}_{BC}^{-1}
C T = T B C ⋅ T B ⋅ T B C − 1
因为上面的变换都是 坐标系的变换 ,所以矩阵相乘 从左到右,即 矩阵右乘
编程库实践
下面通过示例代码对自己使用过的库进行介绍。
Eigen
Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.
Eigen::Matrix3d m3_r_z = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Eigen::Quaterniond q_r_z(m3_r_z);
Eigen::Vector3f v3_translation(x, y, z);
Eigen::Quaternion<double> q(w, qx, qy, qz);
Eigen::Matrix3f m3_rotation = q.matrix();
Eigen::Matrix4f m4_transform = Eigen::Matrix4f::Identity();
m4_transform.block<3,1>(0,3) = v3_translation;
m4_transform.block<3,3>(0,0) = m3_rotation;
TooN
Tom’s Object-oriented numerics library, is a set of C++ header files which provide basic linear algebra facilities
Array2SE3 :
#include <TooN/TooN.h>
#include <TooN/se3.h>
/**
* @brief transform array to TooN::SE3
* @param array array of 3x4 row-major matrix of RT
* @param se3 TooN::SE3 object
*/
void Tools::Array2SE3(const float *array, SE3<> &se3)
{
Matrix<3,3> m3Rotation;
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
m3Rotation[i][j] = array[i*4+j];
}
}
SO3<> so3 = SO3<>(m3Rotation);
Vector<3> v3Translation;
v3Translation[0] = array[ 3];
v3Translation[1] = array[ 7];
v3Translation[2] = array[11];
se3.get_rotation() = so3;
se3.get_translation() = v3Translation;
}
Sophus
C++ implementation of Lie Groups using Eigen commonly used for 2d and 3d geometric problems (i.e. for Computer Vision or Robotics applications)
#include <iostream>
#include <sophus/se3.hpp>
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Eigen::Quaterniond q(R);
Eigen::Vector3d t(1,0,0);
Sophus::SE3 SE3_Rt(R, t);
Sophus::SE3 SE3_qt(q, t);
typedef Eigen::Matrix<double,6,1> Vector6d;
Vector6d se3 = SE3_Rt.log();
std::cout << "se3 hat = " << std::endl
<< Sophus::SE3::hat(se3) << std::endl;
std::cout <<"se3 hat vee = " << std::endl
<< Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose() << std::endl;
Vector6d update_se3;
update_se3.setZero();
update_se3(0,0) = 1e-4d;
Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt;
std::cout << "SE3 updated = " << std::endl
<< SE3_updated.matrix() << std::endl;
ROS tf & tf2
tf is a package that lets the user keep track of multiple coordinate frames over time. tf maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
#include <Eigen/Geometry>
#include <tf_conversions/tf_eigen.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <geometry_msgs/TransformStamped.h>
tf::Transform transform;
transform.setOrigin( tf::Vector3(x, y, z) );
tf::Quaternion q;
q.setRPY(r, p, y);
transform.setRotation(q);
geometry_msgs::Quaternion q_msg;
Eigen::Vector3d v3_r;
tf2::Matrix3x3(tf2::Quaternion(q_msg.x, q_msg.y, q_msg.z, quaternion_imu_.w))
.getRPY(v3_r[0], v3_r[1], v3_r[2]);
tf2::Quaternion q_tf2;
q_tf2.setRPY(v3_r[0], v3_r[1], v3_r[2]);
q_tf2.normalize();
geometry_msgs::TransformStamped tf_stamped;
tf_stamped.transform.rotation.x = q.x();
tf_stamped.transform.rotation.y = q.y();
tf_stamped.transform.rotation.z = q.z();
tf_stamped.transform.rotation.w = q.w();