SLAM1

2024年06月21日 2.5k 字大概 13 分钟

向量和坐标
欧式变换
- [[#欧式变换#基本旋转与平移|基本旋转与平移]]
- [[#欧式变换#齐次坐标|齐次坐标]]
- [[#欧式变换#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(R) = 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

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

基本使用方法

// 求解方程

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(R)-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(R) = 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$

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

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