SLAM1

• 向量和坐标
• 欧式变换
  • [[#欧式变换#基本旋转与平移|基本旋转与平移]]
  • [[#欧式变换#齐次坐标|齐次坐标]]
  • [[#欧式变换#Eigen|Eigen]]
  • [[#欧式变换#旋转矢量|旋转矢量]]

基础数学知识补习

向量和坐标

任意向量$\vec{a}$在基$(e_1,e_2,e_3)$的坐标为

$$\vec{a} = [\vec{e_1},\vec{e_2},\vec{e_3}]\begin{bmatrix}a_1\\a_2\\a_3 \end{bmatrix} = a_1\vec{e_1} + a_2\vec{e_2} + a_3\vec{e_3}$$ 左手坐标系： 通过矢量积$\vec{x}\times\vec{y}$ 的结果沿z方向

$$\vec{x}\times\vec{y} = \begin{vmatrix}e_1 & e_2 &e_3\\a_1 & a_2 & a_3\\b_1&b_2&b_3\end{vmatrix}= \begin{bmatrix}a_2b_3-a_3b_2\\a_3b_1-a_1b_3\\a_1b_2-a_2b_1\end{bmatrix} = \begin{bmatrix}0&-a_3&a_2\\a_3&0&a_1\\a_2&a_1&0\end{bmatrix}b=a^\wedge b$$

欧式变换

旋转加平移——刚体运动

基本旋转与平移

$$[e_1,e_2,e_3]\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=[e_1’,e_2’,e_3’]\begin{bmatrix}a_1’\\a_2’\\a_3’\end{bmatrix}$$ 刚体运动，坐标基矢与该坐标系下的矢量的内积是不变量。

旋转矩阵

$$\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=[e_1,e_2,e_3]^T[e_1’,e_2’,e_3’]\begin{bmatrix}a_1’\\a_2’\\a_3’\end{bmatrix}=\begin{bmatrix}e_1^Te_1’&e_1^Te_2’&e_1^Te_3’\\e_2^Te_1’&e_2^Te_2’&e_2^Te_3’\\e_3^Te_1’&e_3^Te_2’&e_3^Te_3’\end{bmatrix}=Ra’$$

两个坐标系的基的内积构成旋转矩阵，两个向量的积等于其夹角的余弦值也称为方向余弦矩阵。他是行列式为1的正交矩阵。n维旋转矩阵构成的集合为一个特殊正交群(Special Orthogonal Group)。转置矩阵为原矩阵的相反旋转。

$$SO(n) = {R\in \mathbb{R}^{n\times n}|RR^T=I,det® = 1}$$ 平移 在原矢量上加上一个向量即可 欧式坐标变换

$$a’ = Ra+t$$ 坐标系的下表从右向左读，如下式中为从2到1的旋转矩阵

$$a_1=R_{12}a_2+t_{12}$$

齐次坐标

$$\begin{bmatrix}a’\\1\end{bmatrix} = \begin{bmatrix}R&t\\0^T&1\end{bmatrix}\begin{bmatrix}a\\1\end{bmatrix} = T\begin{bmatrix}a\\1\end{bmatrix}$$

在每一个三维向量的末尾添加1，就可以变成齐次坐标计算。T为变换矩阵。

$$\widetilde{b} = T_1 \widetilde{a}, \widetilde{c} = T_2 \widetilde{b}, \widetilde{c} = T_1T_2 \widetilde{a}$$ 特殊欧式群SE()(Special Euclidean Group) 该矩阵的逆为

$$T^{-1} = \begin{bmatrix} R^T & - R^Tt\\0^T &1\end{bmatrix}$$

Eigen

安装eigen

1
2
3
4
5
6

git clone https://github.com/eigenteam/eigen-git-mirror.git
cd eigen-git-mirror
mkdir build
cd build
cmake ..
sudo make install

基本使用方法

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

// 求解方程

Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random (MATRIX_SIZE, MATRIX_SIZE); // 随机生成一个矩阵

matrix_NN = matrix_NN * matrix_NN.transpose(); // X= （AAT），此时X是半正定的。

Matrix<double, MATRIX_SIZE, 1> matrix_b = MatrixXd::Random (MATRIX_SIZE, 1);

clock_t time_start = clock();

// 直接求逆

Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse()*matrix_b;

  

cout << "time consumption of normal inverse is " << 1000*(clock()-time_start)/(double)CLOCKS_PER_SEC << "ms" << endl;

  

cout<< "x = " << endl << x << endl;

  

// 矩阵分解

time_start = clock();

x = matrix_NN.colPivHouseholderQr ().solve(matrix_b); // 用matrix_NN和matrix_b进行解方程。

cout << "time consumption of Qr decomposition is " << 1000*(clock()-time_start)/(double)CLOCKS_PER_SEC << "ms" << endl;

  

// 正定矩阵 cholesky分解

time_start = clock();

x = matrix_NN.ldlt().solve(matrix_b);

cout << "time consumption of cholesky decomposition is " << 1000*(clock()-time_start)/(double)CLOCKS_PER_SEC << "ms" << endl;

cout<<"x is "<<endl<<x<<endl;

我记得一年级的时候老师反复强调求解线性方程组的时候，要解方程，不要求逆

$$Fx = b$$ 一定不要通过$x = F^{-1}b$ 去计算而是要直接解方程。 至于QR分解，cholesky分解我属实是忘记了。

旋转矢量

SO(3)有9个量，大于旋转的3个自由度。 T有16个量，大于变换的6自由度。表达方式冗余。

旋转向量：一个轴+一个旋转角——向量方向与轴一致，大小等于旋转角。 设旋转轴的单位向量为n，旋转角为$\theta$，则旋转向量为$n\theta$，其与旋转矩阵R的关系由罗德里格斯公式导出：

$$R = \cos \theta I +(1-\cos \theta)n n^T +\sin\theta n^\wedge$$ 其中n^为向量到反对称矩阵的转换。

转角有

$$\theta = \arccos\frac{tr®-1}{2}$$

对于旋转轴上的向量而言，其为R的特征向量。换句话讲，可以通过求解特征方程再归一化得到旋转轴。

$$a = kn,Ra = a = kn$$

欧拉角

把旋转分为三个绕不同的轴的旋转。如ZYX旋转：

• 偏航角yaw、俯仰角pitch、横滚角roll
• $[r,p,y]^T$三维向量可以形容任何旋转。

万向锁问题

![20240624144310.png] 俯仰角为±90度时，第一次和第三次旋转公用一个轴，丢失一个自由度。

四元数

（避免歧义性又不想带有冗余性）

$$q = q_0+q_1i+q_2j+q_3k$$ $$\begin{equation} \left\{ \begin{array}{lr} i^2 =j^2 =k^2 = -1\\ ij = k, ji = -k\\ jk = i, kj = -i\\ ki=j, ik = -j \end{array} \right. \end{equation}$$ $$q = [s,v]^T,s=q_0,v = [q_1,q_2,q_3]^T$$

单位四元数可以表示任意旋转。

将三维空间的点用一个虚四元数表示，单位四元数q指定旋转。 $$p = [0,x,y,z]^T$$ 旋转后的p’可以表示为 $$p’=qpq^{-1}$$ p’的虚部即为旋转之后的坐标。 四元数乘法可以写成矩阵乘法。 $q=[s,v]^T$

$$q^+=\begin{bmatrix} s& -v^T\\v& sI+v^\wedge\end{bmatrix}, q^\oplus = \begin{bmatrix} s& -v^T\\v& sI-v^\wedge\end{bmatrix}$$ 可证

$$q_1q_2 = q_1^+q_2=q_2^\oplus q_1$$ 此时，

$$p’=q^+p{q^{-1}}=q^+p^+q^{-1}=q^+{q^{-1}}^\oplus p$$ 代入有

$$q^+{(q^{-1})}^\oplus = \begin{bmatrix} 1 & 0 \\ 0^T & vv^T+s^2I+2sv^\wedge+(v^\wedge)^2& \end{bmatrix}$$ 其右下角的元给出了四元数到旋转矩阵的变换关系

$$R = vv^T+s^2I+2sv^\wedge+(v^\wedge)^2$$ 为什么呢？ 因为

$$\begin{bmatrix} 1 & 0 \\ 0^T & vv^T+s^2I+2sv^\wedge+(v^\wedge)^2& \end{bmatrix} [s_p,v_p]^T = \begin{bmatrix} s_p \\ Rv_p\end{bmatrix}$$ 其中v是原来的向量，R是旋转矩阵，对应可得。

$$tr® = tr(vv^T) + 3s^2 + 2s\ tr(v^\wedge) + tr({v^\wedge}^2) = 4s^2-1$$ $$\theta = \arccos(2s^2-1)$$ $$\theta = 2\arccos s$$ 旋转轴:(如果$v_p=v$，得到该向量本身就不变，即为旋转轴，除以模长)

$$[n_x,n_y,n_z]^T = [q_1,q_2,q_3]^T/\sin\frac{\theta}{2}$$

Eigen几何模块计算

四元数计算：使用前要归一化。 Eigen的各种数据类型都有单精度和双精度，必须手动转换，否则会报错。 如果世界坐标系W中有两个坐标系R1，R2。 其中R1的位姿为$q_1 = [0.35,0.2,0.3,0.1]^T,t_1 = [0.3,0.1,0.1]^T$。 R2的位姿为$q_2 = [-0.5,0.4,-0.1,0.2]^T,t_2 = [-0.1,0.5,0.3]^T$。 四元数能够描述旋转角度和旋转轴，因此还需要一个位移矢量t来描述位置 如果p在1中的坐标是$P_1=[0.5,0,0.2]^T$，则p在世界坐标系中的坐标$P_W =T_1^{-1}P_1$ 再将其转换到2的坐标系中，应有$P_2 = T_2P_W$

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

int main(int argc, char **argv){
	Quaterniond q_1 = Quaterniond(0.35, 0.2, 0.3, 0.1);
	Quaterniond q_2 = Quaterniond(-0.5, 0.4, -0.1, 0.2);
	q_1.normalize();
	q_2.normalize();
	Vector3d t_1 = Vector3d(0.3, 0.1, 0.1);
	Vector3d t_2 = Vector3d(-0.1,0.5,0.3);
	Vector3d p = Vector3d(0.5,0,0.2);
	// 生成世界坐标系到两个坐标系各自的变换矩阵
	Isometry3d T1w(q_1);
	Isometry3d T2w(q_2);
	T1w.pretranslate(t_1); // 添加平移量
	T2w.pretranslate(t_2);
	Vector3d p2 = T2w * T1w.inverse() * p;
	cout << "p2 = " << p2.transpose() << endl;

	return 0;
}

可视化

安装pangolin

1
2
3
4
5
6
7
8
9
10
11
12
13
14

git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin

// 安装Pangolin所需依赖项
sudo apt-get install libglew-dev
sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install libepoxy-dev


mkdir build
cd build
cmake ..
make -j$(nproc)
sudo make install