1. 名词概念

1.1 旋转矩阵

3x3矩阵，Eigen::Matrix3d
表达旋转，具有冗余性
必须是正交矩阵，且行列式为1
构成特殊正交群SO(3)
对应李代数so(3)为3x1向量或反对称矩阵【两者等价】
so(3)实际上就是旋转向量构成的空间，指数映射即罗德里格斯公式。
李代数是满射，而不是单射，每次SO(3)对应一个固定的so(3)，但可能有多个so(3)元素对应同一个SO(3)，因为旋转360度与旋转0度等价。
因此旋转矩阵的导数由旋转向量指定，从而在旋转矩阵中进行微积分运算。

1.2 旋转向量

3x1矩阵，Eigen::AngleAxisd
表达旋转，具有奇异性

1.3 欧拉角

3x1矩阵，Eigen::Vector3d
表达旋转，具有奇异性

1.4 四元数

4x1矩阵，Eigen::Quaterniond
表达旋转，无冗余性也无奇异性

1.5 欧氏变换矩阵

4x4矩阵，Eigen::Isometry3d
表达旋转+平移
必须是正交矩阵，且行列式为1
构成特殊欧氏群SE(3)
对应李代数se(3)为6x1向量或反对称矩阵【两者等价】
前三维表示平移【但与变换矩阵中的平移含义不同】，后三维表示旋转
前后顺序无所谓，有时候平移与旋转也会反过来放置，在程序中以结构体放置。

1.6 仿射变换

4x4矩阵，Eigen::Affine3d

1.7 射影变换

4x4矩阵，Eigen::Projective3d

2. 基本思想概念

2.1 齐次坐标——为坐标增加一个维度

此处感谢导师关于多视觉几何的指导～
使用(x, y, z)可以表示一个3维空间的位置坐标，但是只能表现有限距离，而不能表现无穷远处的点。
因此将维度扩展一维，定义一个4维向量(x, y, z, w)来表示一个3维空间的点(x/w, y/w, z/w)，类似于将2维空间的点投影到3维空间，然后按照比例收缩回到原本的2维空间时，按照相似三角形缩小，正好是其它坐标的值除w。
根据该定义方式，对于有限距离处的三维点(x, y, z)，扩展为4维向量(x, y, z, 1)，即对应3维向量(x/1, y/1, z/1)。
而对于无穷远处的点，则将其扩展为4维向量(x, y, z, 0)，对应着无穷大的3维坐标值。

2.2 基于升维概念解方程

对于直线ax + by + c = 0，相当于矩阵[a, b, c]·[x, y, 1]^T=0，即两个向量cos为0，即两个向量正交【垂直的扩展概念】。

因此，对于求解方程组：
a₁x + b₁y + c₁ = 0
a₂x + b₂y + c₂ = 0
相当于：求解与[a₁, b₁, c₁]，[a₂, b₂, c₂]两个向量同时正交的第三个向量，即这两个向量的叉乘【向量积】，坐标系满足右手定则，得到：
[b₁c₂-c₁b₂, c₁a₂-a₁c₂, a₁b₂,-b₁a₂]

2.3 BA(Bundle Adjustment)

Bundle Adjustment问题：把相机和三维点放在一起进行量最小化的问题。
通过最小化重投影误差求解PnP问题。

3. 坐标变换

3.1 坐标常用写法

参考书籍：《视觉SLAM十四讲-从理论到实践》——高翔

定义坐标系1，2
向量p在两个坐标系下的坐标分别为p₁， p₂
旋转矩阵为R，平移向量为t
因此，满足：
p₁ = R₁₂ * a₂ + t₁₂
R₁₂：将坐标系2的向量变换到坐标系1中。
向量乘在矩阵右侧，因此下标从右读到左。
如果竖着写，则是从下方变换到上方（论文常用写法）。

t₁₂：将坐标系2的向量变换到坐标系1中，即从坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标。
因此，含义为“从1到2的向量”，也是“将坐标系2的原点坐标变换到坐标系1下时的坐标”。
并不是将物体从原点2移动到原点1！而是用坐标系1的坐标表示原点2！
t₁₂和t₂₁并非数值上的取反，因为和两个坐标系的旋转有关系。

变换矩阵T₁₂表示从2到1的变换，同时包括旋转和平移。

通常所说的机器人R的位姿q与平移向量t，指的是q_wr与t_wr（by导师），即从机器人坐标系变换到世界坐标系，但也有一些相反的定义（比如视觉SLAM十四讲上的例题，但题目中会明说定义）。

O：坐标原点
考虑机器人参考系的坐标原点O_R，此时O_W即为机器人坐标系的原点在世界坐标系中的位置：
O_w = T_WR * O_R = t_WR
因此机器人在世界中的坐标即为T_WR中的平移向量的部分。

T_WR与T_RW互为逆矩阵。

同时默认：
_W：世界坐标系
_R：机器人坐标系
p：从某个坐标系中观察到的某个位置向量
q：四元数表示的位姿
t：平移向量

3.2 相对坐标转换计算

【视觉SLAM十四讲上的例题，此处q和t的定义与常用相反】
q为旋转四元数，t为平移向量， p为坐标， T为变换矩阵
1号相对世界：q_1w, t_1w
2号相对世界：q_2w, t_2w
已知目标物体在1号坐标系下的坐标p₁，求它在2号坐标系的坐标

第1步，对四元数进行单位化
第2步，创建欧氏变换矩阵T_2w，T_2w，将对应的q与t装入，可以在创建T的时候直接放入Q作为初始化参数
第3步，计算T_2w * T_1w^-1 * p₁，得到p₂

第3步算式详细推导过程：
首先列出方程组：
Pw = T_w1P₁
T_w1 = T_1w^-1
P₂ = T_2wP_w
代入可得：
p₂ = T_2w * T_1w^-1 * p₁

3.3 线性方程组解法

1、对系数矩阵进行SVD分解
2、取V的最后一列作为通解

3.4 左手系与右手系转换的一个问题

有多个右手系的位姿，用四元数xy取反，平移z取反转换为左手系坐标，但出现了一些问题，比如本来是外八字的箭头，但是在转换后，变成了内八字箭头，实验发现对旋转矩阵乘矩阵
np.array([
[1, 0, 0],
[0, -1, 0],
[0, 0, -1]
])
就可以重新翻转手性，解决这样的转换后方向的偏差。

4. slam中损失函数(Loss function)与代价函数(Cost function)

slam中的cost function即深度学习中的loss function，用于最小化某个函数的值。
slam中的loss function指鲁棒核函数，详情可以参考ceres官方文档中的loss function，例如Huber loss，一般用于减少异常点对结果造成的影响。

5. 小要点

1、棋盘格标定法需要用长方形棋盘，而不能使用正方形棋盘。
2、在引入imu的情况下，右扰动的求导式子比左扰动的式子更加简洁。
3、向量叉乘时，交换位置要加一个负号。