【SLAM】3三维刚体运动

• ref: 《视觉SLAM十四讲从理论到实践 第2版》
  

  文章目录

  • 3三维刚体运动
  • • 3.1旋转矩阵
    • 3.2旋转向量和欧拉角
    • 3.3四元数
    • 3.4变换
  • 习题
  • 实现

3三维刚体运动

3.1旋转矩阵

• 任意向量$a$在基下有坐标 a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 \boldsymbol{a}=\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[
  a1a2a3" role="presentation" style="position: relative;">a1a2a3$$\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}$$
  \right]=a_{1} \boldsymbol{e}_{1}+a_{2} \boldsymbol{e}_{2}+a_{3} \boldsymbol{e}_{3} a=[e1​,e2​,e3​]⎣ ⎡​a1​a2​a3​​⎦ ⎤​=a1​e1​+a2​e2​+a3​e3​
• 内积 $a\cdot b=a^{T}b={\sum }_{i=1}^3{a}_i{b}_i=|a||b|\cos ⟨a,b⟩$
• 外积 a × b = ∥ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ∥ = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b =  def  a ∧ b \boldsymbol{a} \times \boldsymbol{b}=\left\|
  e1e2e3a1a2a3b1b2b3" role="presentation">e1a1b1e2a2b2e3a3b3$$\begin{array}{lll}{\mathit{e}}_1& {\mathit{e}}_2& {\mathit{e}}_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}$$
  \right\|=\left[
  a2b3−a3b2a3b1−a1b3a1b2−a2b1" role="presentation" style="position: relative;">a2b3−a3b2a3b1−a1b3a1b2−a2b1$$\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}$$
  \right]=\left[
  0−a3a2a30−a1−a2a10" role="presentation" style="position: relative;">0a3−a2−a30a1a2−a10$$\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}$$
  \right] \boldsymbol{b} \stackrel{\text { def }}{=} \boldsymbol{a}^{\wedge} \boldsymbol{b} a×b=∥ ∥​e1​a1​b1​​e2​a2​b2​​e3​a3​b3​​∥ ∥​=⎣ ⎡​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​⎦ ⎤​=⎣ ⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦ ⎤​b= def a∧b
  • 外积大小 $|a||b|\sin ⟨a,b⟩$
  • 反对称阵 a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] \boldsymbol{a}^{\wedge}=\left[
    0−a3a2a30−a1−a2a10" role="presentation" style="position: relative;">0a3−a2−a30a1a2−a10$$\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}$$
    \right] a∧=⎣ ⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦ ⎤​
• 旋转矩阵
  • $\rightleftarrows$ 行列式=1的正交阵 (逆=转置)
  • n维旋转矩阵的集合 S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\{R\in \mathbb{R}^{n\times n}|RR^T=I,det(R)=1 \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
• 平移矩阵 $a_1=R_{12}{a}_2+t_{12}$
  • $R_{12}$ :2->1
  • $t_{12}$ :从1到2的向量
• 变换矩阵 T = [ R t 0 T 1 ] T=\left[
  Rt0T1" role="presentation" style="position: relative;">R0Tt1$$\begin{array}{ll}\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
  \right] T=[R0T​t1​]
  • 齐次坐标的转换 $c=Ta$
  • 特殊欧氏群 SE ⁡ ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } \operatorname{SE}(3)=\left\{\boldsymbol{T}=\left[
    Rt0T1" role="presentation" style="position: relative;">R0Tt1$$\begin{array}{ll}\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
    \right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
  • T − 1 = [ R T − R T t 0 T 1 ] \boldsymbol{T}^{-1}=\left[
    RT−RTt0T1" role="presentation" style="position: relative;">RT0T−RTt1$$\begin{array}{cc}{\mathit{R}}^{\mathrm{T}}& -{\mathit{R}}^{\mathrm{T}}\mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
    \right] T−1=[RT0T​−RTt1​]

3.2旋转向量和欧拉角

• 旋转向量: 方向=旋转轴, 长度=旋转角度 一个三维向量即可描述旋转
• 一次变换: 1旋转向量+1平移向量 六维
• 旋转向量->旋转矩阵: $\text{R}=\cos \theta \text{I}+(1-\cos \theta ){\text{nn}}^T+\sin \theta n\stackrel{ˆ}{}$ 单位长度向量n
• 旋转矩阵->旋转向量: $\text{Rn}=\text{n}$
• 欧拉角(yaw-pitch-roll ZYX转角)
• 万向锁: ZYX转角定义下, pitch=90°时, 第三次旋转与第一次旋转相同, 丢失了一个自由度(逻辑旋转顺序和实际旋转顺序不一样)

3.3四元数

• 四元数 $\text{q}=q_0+q_q\text{i}+q_2\text{j}+q_3\text{k}$       ~~~~~       $\text{q}=[s,v{]}^T$
  • 虚部=0:实四元数 实部=0:虚四元数
• 运算
  • 加法 ${\text{q}}_a\pm {\text{q}}_b=[s_a\pm s_b,{\text{v}}_a\pm {\text{v}}_b{]}^T$
  • 乘法 q a q b = s a s b − x a x b − y a y b − z a z b + ( s a x b + x a s b + y a z b − z a y b ) i + ( s a y b − x a z b + y a s b + z a x b ) j + ( s a z b + x a y b − y a x b + z a s b ) k
    qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k" role="presentation" style="position: relative;">qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k$$\begin{array}{rl}{\mathit{q}}_a{\mathit{q}}_b=& s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +\left(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b\right)\mathrm{i}\\ & +\left(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b\right)\mathrm{j}\\ & +\left(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b\right)\mathrm{k}\end{array}$$
    qa​qb​=​sa​sb​−xa​xb​−ya​yb​−za​zb​+(sa​xb​+xa​sb​+ya​zb​−za​yb​)i+(sa​yb​−xa​zb​+ya​sb​+za​xb​)j+(sa​zb​+xa​yb​−ya​xb​+za​sb​)k​
    • ${\mathbf{q}}_a{\mathbf{q}}_b={\left[s_a{s}_b-{\mathbf{v}}_a^{\mathrm{T}}{\mathbf{v}}_b,s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\right]}^{\mathrm{T}}$
  • 模长 $\Vert {\mathbf{q}}_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$
    • $\Vert {\mathbf{q}}_a{\mathbf{q}}_b\Vert =\Vert {\mathbf{q}}_a\Vert \Vert {\mathbf{q}}_b\Vert$ 四元数乘积模=模乘积
  • 共轭 ${\mathbf{q}}_a^{\ast }=s_a-x_a\mathrm{i}-{\mathrm{y}}_{\mathrm{a}}\mathrm{j}-{\mathrm{z}}_{\mathrm{a}}\mathrm{k}={\left[{\mathrm{s}}_{\mathrm{a}},-{\mathrm{v}}_{\mathrm{a}}\right]}^{\mathrm{T}}$
    • 共轭x本身=实四元数 ${\mathbf{q}}^{\ast }\mathbf{q}=\mathbf{q}{\mathbf{q}}^{\ast }={\left[s_a^2+{\mathbf{v}}^{\mathrm{T}}\mathbf{v},0\right]}^{\mathrm{T}}$
  • 逆 ${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/\Vert \mathbf{q}{\Vert }^2$
    • 四元数x其逆=1 $q{q}^{-1}=q^{-1}q=1$
    • 单位四元数: 逆=共轭
    • ${\left({\mathbf{q}}_a{\mathbf{q}}_b\right)}^{-1}={\mathbf{q}}_b^{-1}{\mathbf{q}}_a^{-1}$
  • 数乘 $k\text{q}=[ks,kv{]}^T$
• 旋转 三维空间点$p=[0,x,y,z{]}^T=[0,v{]}^T$
  • 单位四元数才能表示旋转
  • ${\text{p}}^{\prime }={\text{qpq}}^{-1}$
    • p’纯虚四元数,虚部即坐标
• 四元数->旋转矩阵: $R=v{v}^T+s^{2}I+2sv\stackrel{ˆ}{}+(v\stackrel{ˆ}{}{)}^2$
  • $\theta =2\arccos q_0$
  • $[n_x,n_y,x_z]T=[q_1,q_2,q_3{]}^T/\sin \frac{\theta }{2}$

3.4变换

• 相似变换: 比欧式变换多一个自由度, 允许物体均匀缩放
  • T S = [ s R t 0 T 1 ] \boldsymbol{T}_{S}=\left[
    sRt0T1" role="presentation" style="position: relative;">sR0Tt1$$\begin{array}{ll}s\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
    \right] TS​=[sR0T​t1​]
  • 相似变换群: 相似变换的集合 Sim(3)
• 仿射变换: 正交投影, A为可逆矩阵, 立方体不再为方, 各面仍为平行四边形
  • T A = [ A t 0 T 1 ] \boldsymbol{T}_{A}=\left[
    At0T1" role="presentation" style="position: relative;">A0Tt1$$\begin{array}{ll}\mathit{A}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
    \right] TA​=[A0T​t1​]
• 射影变换: 真实世界->相机照片, 2D 8自由度, 3D 15自由度
• 

习题

1. 证明旋转矩阵是正交阵
   1. 正交矩阵: 实施向量长度,内积,距离,夹角等性质都不变的变换的矩阵
   2. 满足$A{A}^T=A^{T}A=E$即为正交阵
   3. $A^T=A^{-1}$, $|A|=\pm 1$
2. 四元数旋转为纯虚四元数
   1. 四元数旋转 $p^{\prime }=qp{q}^{-1}$
      1. $p$为纯虚四元数, $p^{\prime }$也是
3. 转换关系
   1. 四元数->旋转矩阵

实现

Eigen数据结构

Eigen::Matrix3d     //  旋转矩阵
Eigen::AngleAxisd   //  旋转向量
Eigen::Vector3d     //  欧拉角
Eigen::Quaterniond  //  四元数
Eigen::Isometry3d   //  欧式变换矩阵
Eigen::Affine3d     //  仿射变换
Eigen::Projective3d //  射影变换
1
2
3
4
5
6
7

• 矩阵创建
• 矩阵旋转
• 变换
• 可视化
  • 安装: Pangolin安装及问题
  • 运行例程报错: Palingo缺少libpango_windowing
  • include找不到文件z: Pangolin安装以及快速入门
    main.cpp

#include 
using namespace std;

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 

using namespace Eigen;

#define MATRIX_SIZE 50

string trajectory_file = "../trajectory.txt";   //  在build文件的上一级寻找

void test_matrix();
void test_rotate();
void test_transform();
void test_play();

int main(int argc, char **argv)
{
    // test_matrix();
    // test_rotate();
    // test_transform();
    test_play();
    return 0;
}

void test_matrix()
{
    // definition
    // 2x3 martix
    Matrix matrix_23;
    matrix_23 << 1,2,8,4,2,6;   //  input data
    cout << "matrix 2x3 1->6: \n" << matrix_23 << endl;
    // 3x1 martix
    Vector3d v_3d;
    Matrix vd_3d;
    vd_3d << 1,2,3;
    cout << "matrix 3x1 1->3: \n" << vd_3d << endl;
    // 3x3 0s
    Matrix3d matrix_33 = Matrix3d::Zero();
    cout << "matrix 3x3 0: \n" << matrix_33 << endl;
    // not sure size
    Matrix matrix_dynamic;
    MatrixXd matrix_x;
    // matrix_dynamic << 1,2,3,4,5,6,7,8,9;
    // cout << "matrix xxx 12345: \n" << matrix_dynamic << endl;

    // operation
    // visit the elements
    cout<<"matrix 2x3 1->6"<result = matrix_23.cast() * v_3d;
    cout << "[1,2,3;4,5,6]*[3;2,1]=" << result << endl;
    
    // calculation
    matrix_33 = Matrix3d::Random();
    cout << "random matrix: \n" << matrix_33 << endl;
    cout << "transpose: \n" << matrix_33.transpose() << endl;
    cout << "sum: \n" << matrix_33.sum() << endl;
    cout << "trace: \n" << matrix_33.trace() << endl;
    cout << "times 10: \n" << 10 * matrix_33 << endl;
    cout << "inverse: \n" << matrix_33.inverse() << endl;
    cout << "det: \n" << matrix_33.determinant() << endl;
    // eigen
    SelfAdjointEigenSolver eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "Eigen values: \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors: \n" << eigen_solver.eigenvectors() << endl;
    // solve formula
    Matrix matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose();
    Matrixv_Nd = MatrixXd::Random(MATRIX_SIZE, 1);
    // direct inverse
    clock_t time_stt = clock();
    Matrix x = matrix_NN.inverse() * v_Nd;
    cout << "x = " << x.transpose() << " time - " << 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC <> poses)
{
    pangolin::CreateWindowAndBind("Viewer",1024,768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
        pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0)
    );
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0,1.0,0.0,1.0,-1024.0f/768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
    while (pangolin::ShouldQuit() == false)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        glLineWidth(2);
        for(size_t i=0; i> poses;
    ifstream fin(trajectory_file);  //  读取坐标文件
    if(!fin){
        cout << "fail to open";
    }
    while(!fin.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));   //  +四元数
        Twr.pretranslate(Vector3d(tx,ty,tz));   //  +T坐标
        poses.push_back(Twr);   //  放到后边
    }
    cout << "read" << poses.size() << "poses" << endl;
    DrawTrajectory(poses);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230

CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
project(test_slam)

add_executable(test_slam main.cpp)

install(TARGETS test_slam RUNTIME DESTINATION bin)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE "Release")
# set(CMAKE_CXX_FLAGS "-O3")

find_package(Pangolin REQUIRED)

include_directories("/usr/include/eigen3")
include_directories(${Pangolin_LIBRARIES})

target_link_libraries(test_slam ${Pangolin_LIBRARIES})
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16