特征点相关

特征点由关键点和描述子构成：

• 关键点：特征点在图像里的位置
• 描述子：通常是一个向量，描述了该关键点周围的信息，朝向大小等

[ORB-SLAM2] ORB-SLAM中的ORB特征（提取）https://zhuanlan.zhihu.com/p/61738607

ORB剔除匹配点

• 词袋搜索时，利用角度变化直方图剔除误匹配点。
• RANSAC算法，根据重投影误差确定内点最多的模型。

ORB描述子中的O、R、B分别代表以下含义：

• O：指的是“Oriented”，即带方向。在ORB算法中，每个特征点具有方向信息，即该特征点所在位置的主要梯度方向。
• R：指的是“Rotated”，即旋转。在ORB算法中，为了增强算法的鲁棒性，在计算BRIEF描述子之前，会根据特征点的方向信息对其周围的像素进行旋转。
• B：指的是“Binary”，即二进制。在ORB算法中，使用二进制字符串来表示BRIEF描述子，以便于快速匹配和存储。

什么是ORB描述子

ORB（Oriented FAST and Rotated BRIEF）是一种流行的特征点描述子，它是由Ethan Rublee等人在2011年提出的。ORB描述子结合了FAST角点检测器和BRIEF二进制描述子，并添加了方向信息。

ORB描述子的生成过程如下：

1. 在FAST角点检测器中选取一个特征点。
2. 计算该特征点周围一定范围内像素值的变化情况，并选择一个具有较大变化的方向作为该特征点的方向。
3. 以该特征点为中心，计算一组BRIEF描述子。每个描述子由若干个像素对之间的比较结果组成，这些像素对通常是在特征点周围一个固定的窗口内随机选择的。

相比于其他特征描述子，ORB描述子具有以下优点：

1. 计算量小：ORB使用二进制描述子，计算速度非常快。
2. 鲁棒性强：ORB采用FAST角点检测器，可以在不同光照、旋转、尺度等条件下检测到相同的特征点。
3. 易于实现：ORB的实现简单，开源代码易于获取和修改。
   因此，ORB描述子在计算机视觉领域中被广泛应用于目标检测、图像匹配、SLAM等任务中

ORB和FAST的区别是什么？

ORB是特征点描述子，FAST是关键点，使用的时候先用FAST将关键点检测出来，再使用ORB对关键点特征进行描述。
在ORB-SLAM中，FAST与ORB都是改进过的，具体表现如下：
FAST：1. 非极大值抑制用于防止关键点过于扎堆；2. 计算关键点Harris响应值，并排序，选择特征值前N大的关键点，约束了关键点数量；3. 使用图像金字塔，保证尺度不变性；4. 使用几何中心与灰度中心组成的向量，保证尺度不变性。
ORB：1. 使用图像金字塔，保证尺度不变性；2. 使用几何中心与灰度中心组成的向量，保证尺度不变性。

ORB中的初始化过程

1. 选取两个可以作为起始两帧的初始帧
   选取是两个特征点数目大于100的两个连续帧，并进行匹配，只有当前后帧匹配点对比较多时（代码>100），才认为这两帧可以进行初始化并记录下来两帧的匹配关系

2. 根据匹配计算两帧之间的位姿
   同时计算适用于平面场景的单应性矩阵（H）和适用于普通场景的基础矩阵(F),方法是：首先，由抽样点对，计算出单次抽样的H和F矩阵（DLT，八点法）；通过若干次RANSAC抽样，计算出最优的H和F矩阵；然后，通过H和F矩阵的评分，选择最优的矩阵，恢复出来帧间位姿。

3. H和F矩阵评分方式为：

   

4. 三角化测量初始的特征点云深度，进而获得点云地图。
   根据深度值与位姿变换的Z的数值应该相等，构建一个相似三角形，利用深度得到相似比，求解x和像素坐标u以及y和像素坐标v得到方程，执行全局BA来求解和优化位姿。

5. 5.BA优化点云
   在初始化后，单目模式和双目及RGBD模式一样，都是通过PNP来计算位姿。

ORB-SLAM中用到的四叉树：

划分为四个网格，每个网格内检测到的特征点在一定范围内（>1且<25），就继续划分网格，若网格中的特征点≤1，就不在继续划分网格。节点数量超过一定值就不再继续下分网格（30个），最后每个网格中保留response最大的特征点。
划分网格：

处理前后结果对比：

这样就能保证图片的特征点比较均匀。

优化相关

AX=b的求解方法

https://blog.csdn.net/qq_41839222/article/details/96274251

SVD分解
https://blog.csdn.net/u010196944/article/details/127514369

计算机视觉相关

针对不同的条件和环境进行一系列划分：

• 已知 x 、 K 、 R 、 t ，求 X ： 三角化（ Triangulation ）
• 已知 x 、 X 、 K ，求 R 、 t ： 姿态估计（ Pose Estimation ）
• 已知 x 、 X ，求 K 、 R 、 t ： 相机标定（ Camera Calibration ）
• 已知 x ，求 K 、 R 、 t 、 X ： 稀疏重建（ Sparse Reconstruction ）

x：像素坐标，X：3D坐标，R相机旋转矩阵，t相机平移向量，K相机内参矩阵，R和t合起来是相机的外参矩阵T

消影点（vanishing points） ：

空间平行线在图像投影线的交点， 对应三维空间中的无穷远点。消影点只与三维直线方向有关， 与其位置无关。

图像金字塔：

图像金字塔能解决FAST特征点缺乏尺度不变性的问题

（不管原图尺度是多少，在包含了所有尺度的尺度空间下都能找到那些稳定的极值点，这样就做到了尺度不变；即不管距离远或者近，提取看上去都是角点的点）

退化问题：

正常情况下基础矩阵模型应该可以应付，但如果特征点共面（初始化场景中主要是一个平面），或者两帧之间的相对位姿未纯旋转时，基础矩阵的自由度会下降，也就是所谓的退化，类似于方程数少于变量数。此时为了保证运动恢复的精度，就不能再用基础矩阵模型。由此提出了单应矩阵，其假设特征点落在同一平面上，从而适用于这种场景下的运动恢复。

齐次坐标：

简单说来，齐次坐标就是在原有的坐标维度上再添加一个维度。

齐次坐标的优势：

• 方便的表达点在直线或平面上

2D平面上，一条直线l可以用方程ax+by+c=0来表示，该直线用向量来表示记作

l = ( a , b , c ) T l=(a,b,c)^T l=(a,b,c)T

点p的齐次坐标为(x,y,1)，则点P在直线上就可以用内积（点乘）来表示

l T ∗ p = ( a , b , c ) T ∗ ( x , y , z ) = a x + b y + c l^T*p=(a,b,c)^T*(x,y,z)=ax+by+c lT∗p=(a,b,c)T∗(x,y,z)=ax+by+c

若点在直线上则 a x + b y + c = 0 ax+by+c=0 ax+by+c=0

• 方便表达直线与直线，平面与平面的交点

结论：在齐次坐标下，可以用两个点 p, q 的齐次坐标叉乘结果来表达一条直线 l，也就是 l = p × q l = p \times q l=p×q

也可以使用两条直线 l ( a x , a x , a z ) l(a_x,a_x,a_z) l(ax​,ax​,az​), m ( b x , b y , b z ) m(b_x,b_y,b_z) m(bx​,by​,bz​)的叉乘表示他们的交点 x

x = l × m = ( a x , a y , a z ) T × ( b x , b y , b z ) = ∣ i j k a x a y a z b x b y b z ∣ = i ∣ a y a z b y b z ∣ − j ∣ a x a z b x b z ∣ + k ∣ a x a y b x b y ∣ x = l \times m=(a_x,a_y,a_z)^T \times (b_x,b_y,b_z)\\= \begin {vmatrix} i&j&k\\ a_x&a_y&a_z\\ b_x&b_y&b_z \end{vmatrix} =i\begin {vmatrix} a_y&a_z\\ b_y&b_z \end{vmatrix} -j\begin {vmatrix} a_x&a_z\\ b_x&b_z \end{vmatrix} +k\begin{vmatrix} a_x&a_y\\b_x&b_y \end{vmatrix} x=l×m=(ax​,ay​,az​)T×(bx​,by​,bz​)= ​iax​bx​​jay​by​​kaz​bz​​ ​=i ​ay​by​​az​bz​​ ​−j ​ax​bx​​az​bz​​ ​+k ​ax​bx​​ay​by​​ ​

• 能够区分一个向量和一个点：

1. 从普通坐标转换成齐次坐标时

如果(x,y,z)是个点，则变为(x,y,z,1);
如果(x,y,z)是个向量，则变为(x,y,z,0)

2.从齐次坐标转换成普通坐标时

如果是(x,y,z,1)，则知道它是个点，变成(x,y,z);
如果是(x,y,z,0)，则知道它是个向量，仍然变成(x,y,z)

• 能够表达无穷远
  如果一个点的齐次坐标中，最后一个元素为0，则表示为无穷远点。
• 更简洁的表达欧氏空间变换
  使用齐次坐标，可以方便的将加法转化为乘法，方便的表达平移。

向量点乘定义：

a ∗ b = ∣ ∣ a ∣ ∣ ∗ ∣ ∣ b ∣ ∣ ∗ c o s ( θ ) a * b = ||a||* ||b|| *cos(θ) a∗b=∣∣a∣∣∗∣∣b∣∣∗cos(θ)

叉乘定义：

a × b = ∣ ∣ a ∣ ∣ ∗ ∣ ∣ b ∣ ∣ ∗ s i n ( θ ) ∗ n a \times b = ||a|| *||b|| *sin(θ)*n a×b=∣∣a∣∣∗∣∣b∣∣∗sin(θ)∗n

n为一个与向量a,b所构成的平面垂直的单位向量

相机模型：

参考https://www.cnblogs.com/fantasyzzz/p/15213328.html

如上图所示，是一个小孔成像的模型，其中：

• C点表示camera centre，即相机的中心点，也是相机坐标系的中心点，光心；
• Z 轴表示principal axis，即相机的主轴；
• p 点所在的平面表示image plane，即相机的成像平面，也就是图片坐标系所在的二维平面；
• p点表示principal point，即主点，主轴与成像平面相交的点；
• C 点到 p 点的距离，也就是右边图中的 f 表示focal length，即相机的焦距；
• 相机坐标系是以 X， Y， Z（大写）三个轴组成的且原点在 C 点，度量值为米（m）；
• 成像平面坐标系是以 x，y（小写）两个轴组成的且原点在 p 点，度量值为米（m）；

图像坐标系一般指图片相对坐标系，在这里可以认为和像平面坐标系在一个平面上，不过原点是在图片的角上，而且度量值为像素的个数（pixel）；

相机的内参矩阵：

K = ∣ f x 0 c x 0 f y c y 0 0 1 ∣ K=\left|\begin{array}{ccc}f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1\end{array}\right| K= ​fx​00​0fy​0​cx​cy​1​ ​

其中 c x , c y c_x,c_y cx​,cy​为相机光轴到图像原点的偏移量 f x , f y f_x,f_y fx​,fy​为根据焦距得到的物体到相机光心的比例关系：

f x = f d x , f y = f d y f_x=\frac{f}{dx}, f_y=\frac{f}{dy} fx​=dxf​,fy​=dyf​

dx和dy表示x方向和y方向的一个像素分别占多少长度单位，即一个像素代表的实际物理值的大小

相机坐标系到图像坐标系下的转换为：

s [ u v 1 ] = K [ x c y c z c ] s\begin{bmatrix}u\\v\\1\end{bmatrix} =K\begin{bmatrix}{x_c}\\{y_c}\\{z_c}\end{bmatrix} s ​uv1​ ​=K ​xc​yc​zc​​ ​

其中，s为尺度因子一般取深度值化作归一化平面下的齐次坐标，去掉尺度因子，转化为等式，图像坐标系下像素点(u,v)与图像坐标系下点(x,y)的投影关系

u = f x x + c x v = f y y + c y u=f_xx+c_x\\ v=f_yy+c_y u=fx​x+cx​v=fy​y+cy​

这里的x,y为归一化坐标即x=X/Z,y=Y/Z

反投影则为：

x = u − c x f x y = u − c y f y x=\frac{u-c_x}{f_x}\\ y=\frac{u-c_y}{f_y} x=fx​u−cx​​y=fy​u−cy​​

畸变模型

在几何光学和阴极射线管(CRT)显示中，畸变（distortion） 是对直线投影（rectilinear projection）的一种偏移。简单来说直线投影是场景内的一条直线投影到图片上也保持为一条直线。那畸变简单来说就是一条直线投影到图片上不能保持为一条直线了，这是一种光学畸变（optical aberration）。可能由于摄像机镜头的原因，畸变一般可以分为两大类，包括径向畸变和切向畸变。主要的一般径向畸变有时也会有轻微的切向畸变。

径向畸变（Radial distortion）

径向畸变的效应有三种，一种是桶形畸变（barrel distortion），另一种是枕形畸变（pincushion distortion），还有一种是两种的结合叫做胡子畸变（mustache distortion），从图片中可以很容易看出区别，具体见下图(图片来自wikipedia)：

径向畸变矫正公式：

x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + + k 2 r 4 + k 3 r 6 ) x_{distorted}=x(1+k_1r^2+k_2r^4+k_3r^6)\\ y_{distorted}=y(1+k_1r^2++k_2r^4+k_3r^6) xdistorted​=x(1+k1​r2+k2​r4+k3​r6)ydistorted​=y(1+k1​r2++k2​r4+k3​r6)

切向畸变（tangential distortion）

切向畸变是由于透镜与成像平面不严格的平行，其可以用如下公式修正：

x d i s t o r t e d = x + [ 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) ] y d i s t o r t e d = y ( 1 + k 1 r 2 + + k 2 r 4 + k 3 r 6 ) x_{distorted}=x+[2p_1xy+p_2(r^2+2x^2)]\\ y_{distorted}=y(1+k_1r^2++k_2r^4+k_3r^6) xdistorted​=x+[2p1​xy+p2​(r2+2x2)]ydistorted​=y(1+k1​r2++k2​r4+k3​r6)

其中 r 2 = x 2 + y 2 r^2=x^2+y^2 r2=x2+y2，畸变参数为 k 1 , k 2 , k 3 , p 1 , p 2 k_1,k_2,k_3,p_1,p_2 k1​,k2​,k3​,p1​,p2​

曼哈顿世界假设

https://zhuanlan.zhihu.com/p/190232333
如下图所示，环境中存在垂直/正交的信息，如地板、天花板、墙面等。通过判断相机和曼哈顿世界之间的旋转矩阵，进而可以获得相机与相机之间的相对旋转矩阵。

三角测量（三角化）：

定义：通过在两处观测同一个点的夹角，从而确定该点的距离。
作用：通过三角测量来估计地图点的深度。

具体求解：

已知条件：
相机内参K、外参R、t
目标：
根据2D像素匹配点对求取对应的3D空间点。
3D点X，在相机两个不同位置C1和C2下的坐标为 X C 1 X_{C1} XC1​和 X C 2 X_{C2} XC2​,C1和C2之间的旋转为 R C 1 C 2 R_{C1C2} RC1C2​,平移为 t C 1 C 2 t_{C1C2} tC1C2​，则

X C 2 = R C 2 C 1 X C 1 + t C 2 C 1 X_{C2}=R_{C2C1}X_{C1}+t_{C2C1} XC2​=RC2C1​XC1​+tC2C1​

根据相机投影模型

p = [ u , v , 1 ] T = 1 Z C 2 K X C 2 = 1 Z C 1 K ( R C 2 C 1 X C 1 + t C 2 C 1 ) = [ R C 2 C 1 ∣ t C 2 C 1 ] [ X C 1 1 ] = [ P 1 T P 2 T P 3 T ] [ X C 1 1 ] = P [ X C 1 1 ] P 1 T = [ P 11 , P 12 , P 13 , P 14 ] p=[u,v,1]^T=\frac{1}{Z_{C2}}KX_{C2} =\frac{1}{Z_{C1}}K(R_{C2C1}X_{C1}+t_{C2C1}) \\\ \\=[R_{C2C1}|t_{C2C1}] \begin{bmatrix} X_{C1}\\1 \end{bmatrix} =\begin{bmatrix} P_1^T\\P_2^T\\P_3^T \end{bmatrix} \begin{bmatrix} X_{C1}\\1 \end{bmatrix}=P \begin{bmatrix} X_{C1}\\1 \end{bmatrix} \\\ \ \\ P_1^T=[P_{11},P_{12},P_{13},P_{14}] p=[u,v,1]T=ZC2​1​KXC2​=ZC1​1​K(RC2C1​XC1​+tC2C1​) =[RC2C1​∣tC2C1​][XC1​1​]= ​P1T​P2T​P3T​​ ​[XC1​1​]=P[XC1​1​]  P1T​=[P11​,P12​,P13​,P14​]

两边同时叉乘p

p × p = p × P [ X C 1 1 ] = 0 p\times p=p\times P\begin{bmatrix} X_{C1}\\1 \end{bmatrix}=0 p×p=p×P[XC1​1​]=0

对p取反对称矩阵

p = [ u v 1 ] , [ p ] = [ 0 − 1 v 1 0 − u − v u 0 ] p=\begin{bmatrix}u\\v\\1\end{bmatrix},[p]= \begin{bmatrix} 0&-1&v\\ 1&0&-u\\ -v&u&0\end{bmatrix} p= ​uv1​ ​,[p]= ​01−v​−10u​v−u0​ ​

原式右边化为

[ 0 − 1 v 1 0 − u − v u 0 ] [ P 1 T P 2 T P 3 T ] [ X C 1 1 ] = [ v P 3 T − P 2 T P 1 T − u P 3 T u P 2 T − v P 1 T ] [ X C 1 1 ] = 0 \begin{bmatrix} 0&-1&v\\ 1&0&-u\\ -v&u&0\end{bmatrix} \begin{bmatrix} P^T_1\\ P^T_2\\ P^T_3\end{bmatrix} \begin{bmatrix} X_{C1}\\1 \end{bmatrix} =\begin{bmatrix} vP^T_3-P_2^T\\ P^T_1-uP^T_3\\ uP^T_2-vP^T_1\end{bmatrix}\begin{bmatrix} X_{C1}\\1 \end{bmatrix}=0 ​01−v​−10u​v−u0​ ​ ​P1T​P2T​P3T​​ ​[XC1​1​]= ​vP3T​−P2T​P1T​−uP3T​uP2T​−vP1T​​ ​[XC1​1​]=0

由于第三行可以用第一行和第二行线性表示，因此

[ v P 3 T − P 2 T P 1 T − u P 3 T ] [ X C 1 1 ] = 0 \begin{bmatrix} vP^T_3-P_2^T\\ P^T_1-uP^T_3\end{bmatrix}\begin{bmatrix} X_{C1}\\1 \end{bmatrix}=0 [vP3T​−P2T​P1T​−uP3T​​][XC1​1​]=0

对于 X C 2 X_{C2} XC2​同样可以得到两个方程

p ′ = [ u ′ v ′ 1 ] = 1 Z C 1 K X C 1 = [ P 1 ′ T P 2 ′ T P 3 ′ T ] [ X C 1 1 ] = P ′ T [ X C 1 1 ] p'=\begin{bmatrix}u'\\v'\\1\end{bmatrix}=\frac{1}{Z_{C1}}KX_{C1}= \begin{bmatrix} P'^T_1\\ P'^T_2\\ P'^T_3\end{bmatrix} \begin{bmatrix} X_{C1}\\1 \end{bmatrix}=P'^T\begin{bmatrix} X_{C1}\\1 \end{bmatrix} p′= ​u′v′1​ ​=ZC1​1​KXC1​= ​P1′T​P2′T​P3′T​​ ​[XC1​1​]=P′T[XC1​1​]

同样两边同时对 p ′ p' p′叉乘，得到两个方程

[ v ′ P 3 ′ T − P 2 ′ T P 1 ′ T − u ′ P 3 ′ T ] [ X C 1 1 ] = 0 \begin{bmatrix} v'P'^T_3-P_2'^T\\ P'^T_1-u'P'^T_3\end{bmatrix}\begin{bmatrix} X_{C1}\\1 \end{bmatrix}=0 [v′P3′T​−P2′T​P1′T​−u′P3′T​​][XC1​1​]=0

因此，一对匹配点可得方程组：

[ v P 3 T − P 2 T P 1 T − u P 3 T v ′ P 3 ′ T − P 2 ′ T P 1 ′ T − u ′ P 3 ′ T ] [ X C 1 1 ] = 0 A 4 ∗ 4 X 4 ∗ 1 = 0 4 ∗ 1 \begin{bmatrix} vP^T_3-P_2^T\\ P^T_1-uP^T_3\\ v'P'^T_3-P_2'^T\\ P'^T_1-u'P'^T_3\end{bmatrix}\begin{bmatrix} X_{C1}\\1 \end{bmatrix}=0\\ A_{4*4}X_{4*1}=0^{4*1} ​vP3T​−P2T​P1T​−uP3T​v′P3′T​−P2′T​P1′T​−u′P3′T​​ ​[XC1​1​]=0A4∗4​X4∗1​=04∗1

求解该线性方程组，便可得3D空间点，在 C 1 C_1 C1​坐标系下的坐标。

ORB-SLAM2三角化代码：

//P1为内参矩阵K P2为K(R|t)
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{cv::Mat A(4,4,CV_32F);A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);cv::Mat u,w,vt;cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);x3D = vt.row(3).t();//取SVD分解得到的V矩阵最后一列作为$P_1$的解x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

位姿估计：

2D-2D 对极几何：

左右两个平行四边形分别是相机在不同位置的成像平面

C0, C1分别是两个位置中相机的光心，也就是针孔相机模型中的针孔。

P是空间中的一个三维点，p0, p1分别是P点在不同成像平面上对应的像素点（通过特征匹配得到）

c0-c1连线在成像平面上的交点为极点

极点和匹配点的连线就是极线

本质矩阵、基础矩阵、单应矩阵之间的区别

对极约束1： p 0 T E p 1 = 0 p^T_0Ep_1=0 p0T​Ep1​=0
本质矩阵： E = t ˆ R E=t_{}\^{}R E=t​ˆR
这里的 p 0 , p 1 p_0,p_1 p0​,p1​为归一化平面的坐标
对极约束2： x 0 T F x 1 = 0 x_0^TFx_1=0 x0T​Fx1​=0
基础矩阵： F = K − T t ˆ R K − 1 F=K^{-T}t_{}\^{}RK^{-1} F=K−Tt​ˆRK−1
这里的 x 0 , x 1 x_0,x_1 x0​,x1​为像素坐标
求解等式即可求得相机位姿，其中本质矩阵与相机内参无关。
然而当平移向量为零时，对极几何的基础就不成立了（等式恒为零），此时E、F都无法求解，这时利用单应性矩阵进行位姿的求解。
单应(Homography)是射影几何中的概念，又称为射影变换。它把一个射影平面上的点(三维齐次矢量)映射到另一个射影平面上，并且把直线映射为直线，具有保线性质。总的来说，单应是关于三维齐次矢量的一种线性变换，
可以用一个3×3的非奇异矩阵H表示 x 1 = H x 0 x_1=Hx_0 x1​=Hx0​

区别

基础矩阵F表示的是两视图的对极约束，和三维场景的结构无关，只依赖于相机的内参数以及外参数，需要两个相机的位置旋转和平移
单应矩阵对场景的三维结构有了更多的要求，需要场景中的点在同一个平面上； 或者是，对相机的位姿有了要求，两个相机之间只有旋转而无平移
E矩阵为33且自由度为5的矩阵，R、t有六个自由度，因为尺度等价去掉一个自由度。
F矩阵为33且自由度为7的矩阵，RT6自由度加上一个尺度因子自由度。
H矩阵为33且自由度为8的矩阵，单应矩阵描述的是两个像素坐标间的关系，相比于E,F更加直接！！！
Ep1看做是直线的方程，p0看做是直线上的点，也就是说E*p1就是以C0为原点坐标系中的极线

根据对极几何定义，设x1,x2为特征点的归一化坐标，则他们满足：

s 1 x 1 = s 2 R x 2 + t s_1x_1=s_2Rx_2+t s1​x1​=s2​Rx2​+t

经过对极几何已经求得相机运动R和t，需要求解特征点的深度s1,s2.
两个深度可以分开算，若先算s2,那么对上式两边左乘一个x1^，得：
s 1 x 1 ˆ = s 2 x 1 ˆ R x 2 + x 1 ˆ t = 0 s_1x_1{\^{}}=s_2x_1\^{}Rx_2+x_1\^{}t=0 s1​x1​ˆ=s2​x1​ˆRx2​+x1​ˆt=0
该式左侧为0，右侧为s2的一个方程，可根据它直接求解s2。

代码

特征点匹配的方法可以LK光流/暴力匹配/FLANN
特征点匹配->计算基础矩阵->计算本质矩阵->本质矩阵恢复位姿
恢复位姿代码如下

//-- 计算基础矩阵Mat fundamental_matrix;fundamental_matrix = findFundamentalMat(points1, points2, FM_RANSAC);//cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;//-- 计算本质矩阵Point2d principal_point(300, 300);        //相机主点, TUM dataset标定值int focal_length = 600;            //相机焦距, TUM dataset标定值Mat essential_matrix;essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);//cout << "essential_matrix is " << endl << essential_matrix << endl;//-- 计算单应矩阵Mat homography_matrix;homography_matrix = findHomography(points1, points2, RANSAC, 3);//cout << "homography_matrix is " << endl << homography_matrix << endl;//-- 从本质矩阵中恢复旋转和平移信息.recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);//cout << "R is " << endl << R << endl;//cout << "t is " << endl << t << endl;

单应性矩阵

3D-2D PNP算法

目前遇到的场景主要有两个，
其一是求解相机相对于某2维图像/3维物体的位姿；
其二就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。
在场景1中，我们通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机坐标系相对于世界坐标系(Twc)的位姿
在场景2中，通常输入的是上一帧中的3D点（在上一帧的相机坐标系下表示的点）和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换
算法伪代码：
求解PNP问题的常用算法 DLT（直接线性变换），EPNP（把2D转换为3D点，用ICP求解），P3P（构建三角形），UPNP等

DLT求解：

类似三角化中的DLT求解过程，已知3D点 P = [ X , Y , Z , 1 ] T P=[X,Y,Z,1]^T P=[X,Y,Z,1]T ,2D点像素坐标 s = [ u , v , 1 ] T s=[u,v,1]^T s=[u,v,1]T,
则

s = [ u v 1 ] = [ R ∣ T ] [ X Y Z 1 ] = [ t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ] [ X Y Z 1 ] s=\begin{bmatrix} u\\v\\1 \end{bmatrix} =[R|T] \begin{bmatrix} X\\Y\\Z\\1 \end{bmatrix} =\begin{bmatrix} t_1&t_2&t_3&t_4\\ t_5&t_6&t_7&t_8\\ t_9&t_{10}&t_{11}&t_{12}\\ \end{bmatrix} \begin{bmatrix} X\\Y\\Z\\1 \end{bmatrix} s= ​uv1​ ​=[R∣T] ​XYZ1​ ​= ​t1​t5​t9​​t2​t6​t10​​t3​t7​t11​​t4​t8​t12​​ ​ ​XYZ1​ ​

消去最后一行，得到两个约束

u 1 = t 1 X + t 2 Y + t 3 Z + t 4 t 9 X + t 10 Y + t 11 Z + t 12 u 2 = t 5 X + t 6 Y + t 7 Z + t 8 t 9 X + t 10 Y + t 11 Z + t 12 u_1=\frac{t_1X+t_2Y+t_3Z+t4}{t_9X+t_{10}Y+t_{11}Z+t_{12}}\\ u_2=\frac{t_5X+t_6Y+t_7Z+t8}{t_9X+t_{10}Y+t_{11}Z+t_{12}} u1​=t9​X+t10​Y+t11​Z+t12​t1​X+t2​Y+t3​Z+t4​u2​=t9​X+t10​Y+t11​Z+t12​t5​X+t6​Y+t7​Z+t8​

简化[R|t]的行向量 T = [ t 1 , t 2 , t 3 ] T T=[t_1,t_2,t_3]^T T=[t1​,t2​,t3​]T

可以得到：

t 1 T P − t 3 T P u 1 = 0 t 2 T P − t 3 T P v 1 = 0 t_1^TP-t^T_3Pu_1=0\\ t_2^TP-t^T_3Pv_1=0 t1T​P−t3T​Pu1​=0t2T​P−t3T​Pv1​=0

可以看到每个特征点提供了两个关于t的线性约束。假设一共有N个特征点，则可以列出2N个线性方程组，代求变量T一共有十二个未知数，因此最少通过6对匹配点，即可实现T的线性求解，这种方法成为DLT，匹配点大于6对时，可以使用SVD等方法对超定方程求最小二乘解。

EPNP

EPnP的特点

• EPnP的复杂度是$O(n) $所以对于特征点较多的PnP问题，非常高效。
• 核心思想是将3D点表示为4个控制点的组合，优化也只针对4个控制点，所以速度很快，在求解 Mx=0时，最多考虑四个奇异向量，因此精度也很高。

步骤

1. 世界坐标系下取4个控制点 c j w , j = 1 , 2 , 3 , 4 c_j^w,j=1,2,3,4 cjw​,j=1,2,3,4，理论上可以任取，只要这四个点不共面就行(共面无法组成坐标系)，原论文使用的选取方法是，取所有点的质心 c 1 w c_1^w c1w​为原点，通过主成分分析PCA得到另外三个点再建立坐标系。
2. 已知参考点(特征点)在世界坐标系下的坐标， P i w , i = 1 , 2 … , n P^w_i,i=1,2…,n Piw​,i=1,2…,n，以及控制点在世界坐标系下的坐标，计算权重因子 α \alpha α
3. 计算四个控制点在相机坐标系下的坐标 c j c , j = 1 , 2 , 3 , 4 c_j^c,j=1,2,3,4 cjc​,j=1,2,3,4（核心）
4. 计算参考点在相机坐标系下的坐标 P i c , i = 1 , 2 … n P_i^c,i=1,2…n Pic​,i=1,2…n
5. 根据ICP方法，计算R、t。

算法中的控制点概念

控制点：在三维坐标系中，任意一个3D点都可以由4个不共面的控制点线性表示。

权重因子 α i j \alpha_{ij} αij​是齐次重心坐标。本质上就是：3D特征点的齐次坐标是控制点齐次坐标的线性组合。

3D-3D ICP算法：

SVD求解ICP算法伪代码：
求的是从p2到p1的旋转矩阵和平移向量，也就是从后一帧到前一帧的变换。
1.获取质心坐标p1,p2
2.建立质心坐标系q1,q2，将坐标转化为质心坐标系
3.代入公式，求解 W = q 1 ∗ q 2 T W=q1*q2^T W=q1∗q2T
4.对W进行SVD分解 W = U ∑ V T W=U\sum V^T W=U∑VT，其中 ∑ \sum ∑为奇异值构成的对角矩阵，对角线元素从大到小排列
当W满秩时，旋转矩阵 R = U V T R=UV^T R=UVT，若R的行列式小于零，则R=-R
5.平移向量 t = p 1 − R p 2 t=p1-Rp2 t=p1−Rp2

void pose_estimation_3d3d(const vector<Point3f> &pts1,const vector<Point3f> &pts2,Mat &R, Mat &t) {Point3f p1,p2;int N=pts1.size();for(int i=0;i<N;i++){p1+=pts1[i];p2+=pts2[i];}//获取质心坐标p1=Point3f(Vec3f(p1)/N);p2=Point3f(Vec3f(p2)/N);vector<Point3f> q1(N),q2(N);//以质心坐标为原点的坐标系for(int i=0;i<N;i++){q1[i]=pts1[i]-p1;q2[i]=pts2[i]-p2;}//计算q1*q2^TEigen::Matrix3d W = Eigen::Matrix3d::Zero();for(int i=0;i<N;i++){W+=Eigen::Vector3d(q1[i].x,q1[i].y,q1[i].z) * Eigen::Vector3d(q2[i].x,q2[i].y,q2[i].z).transpose();}//对W进行SVD分解Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | EigenLLComputeFullV);Eigen::Matrix3d U = svd.matrixU();Eigen::Matrix3d V =svd.matrixV(); Eigen::Matrix3d R_=U*(V.transpose());if(R_.determinant()<0){//行列式小于零R_=-R_;}Eigen::Vector3d t_=Eigen::Vector3d(p1.x,p1.y,p1.z)-R_*Eigen::Vector3d(p2.x,p2.y,p2.z);R=(Mat_<double>(3,3) <<R_(0,0),R_(0,1),R_(0,2),R_(1,0),R_(1,1),R_(1,2),R_(2,0),R_(2,1),R_(2,2));t=(Mat_<double>(3,1) << t_(0,0), t_(0,1), t_(0,2));
}

直接法：

直接法具体过程如下：

1. 准备工作。假设相邻帧之间的位姿 Tk,k−1 已知，一般初始化为上一相邻时刻的位姿或者假设为单位矩阵。通过之前多帧之间的特征检测以及深度估计，我们已经知道第k-1帧中特征点位置以及它们的深度。
2. 重投影。知道 Ik−1 中的某个特征在图像平面的位置 (u,v) ，以及它的深度 d ，能够将该特征投影到三维空间 pk−1 ，该三维空间的坐标系是定义在 Ik−1 摄像机坐标系的。所以，我们要将它投影到当前帧 Ik 中，需要位姿转换 Tk,k−1 ，得到该点在当前帧坐标系中的三维坐标 pk 。最后通过摄像机内参数，投影到 Ik 的图像平面 (u′,v′) ，完成重投影。
3. 迭代优化更新位姿 。按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到，它的亮度值应该没啥变化。但由于位姿是假设的一个值，所以重投影的点不准确，导致投影前后的亮度值是不相等的。不断优化位姿使得这个残差最小，就能得到优化后的位姿 Tk,k−1 。
   将上述过程公式化如下：通过不断优化位姿 Tk,k−1 最小化残差损失函数。

直接法的意义

直接法是视觉里程计另一主要分支，它与特征点法有很大不同。虽然它还没有成为现 在 VO 中的主流，但经过近几年的发展，直接法在一定程度上已经能和特征点法平分秋色。

特征点法的缺点：

• 关键点的提取与描述子的计算非常耗时。
• 使用特征点时，忽略了除特征点以外的所有信息。一张图像有几十万个像素，而特征 点只有几百个。只使用特征点丢弃了大部分可能有用的图像信息。
• 相机有时会运动到特征缺失的地方，往往这些地方没有明显的纹理信息。例如，有时 我们会面对一堵白墙，或者一个空荡荡的走廓。这些场景下特征点数量会明显减少， 我们可能找不到足够的匹配点来计算相机运动。

怎样克服这些缺点？

1. **保留特征点，，但只计算关键点，不计算描述子。**同时，使用光流法（Optical Flow） 来跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但光流本身的计算 需要一定时间；
2. **只计算关键点，不计算描述子。**同时，使用直接法（Direct Method）来计算特征点 在下一时刻图像的位置。这同样可以跳过描述子的计算过程，而且直接法的计算更加 简单。
3. 既不计算关键点、也不计算描述子，而是根据像素灰度的差异，直接计算相机运动。

区别

直接法是假设整个图像中的像素点具有相同的运动，根据两帧图像的像素点灰度误差和最小，进行匹配

光流法是对每个像素基于灰度不变原则，在另一帧图像中寻找像素点的匹配。

直接法和光流法是SLAM中常用的两种视觉里程计算法，它们的主要区别在于如何计算相邻帧之间的运动：

1. 光流法：光流法通过追踪图像中的像素点，在相邻帧之间计算其在图像平面上的位移，从而得到相机的运动。光流法通常基于亮度恒定和小运动假设，一般适用于精确的相对运动估计，例如在相机运动缓慢，图像采集频率高等情况下。大运动时，使用金字塔来解决。
2. 直接法：直接法通过对相邻帧之间的图像进行直接匹配，直接计算相机的运动。直接法不依赖于特征点提取，可以同时利用所有像素信息，因此适合在缺乏明显特征纹理的场景下使用，并且更加鲁棒，可以处理大幅度的运动、遮挡、光照变化等情况。

综上所述，光流法和直接法各有优劣，并不是哪一种方法都可以适用于任何情况。在实际应用中，需要根据具体场景选择合适的方法或将它们结合起来使用，以达到更好的性能和效果。

ChatGPT回答的直接法步骤：

SLAM中的直接法通过对相邻帧中的像素灰度值进行匹配来计算相机的运动。其主要步骤如下：

1. 选择参考帧：在相邻帧中，选择一帧作为参考帧，并将其视为静态的背景。
2. 提取关键点：在当前帧中提取一组关键点，例如在像素网格上均匀采样或使用Harris角点检测器等方法。
3. 计算误差：对于每个关键点，在参考帧和当前帧之间利用灰度值信息计算其在当前帧中的位置，并计算这个位置与实际位置之间的误差。常见的误差函数包括灰度误差、绝对值误差、Huber Loss等。
4. 求解运动：利用优化算法（例如最小二乘法）求解相机的运动，以最小化所有关键点的误差。通常采用非线性优化算法（例如Gauss-Newton、Levenberg-Marquardt等）进行求解。

相比于传统的基于特征点的SLAM方法，直接法在处理纹理较弱的场景时表现更好，而且可以同时优化所有像素点，具有更高的精度和鲁棒性。但是，直接法也存在一些问题，例如运动噪声容易引入误差、计算量大、需要大量内存等。

点的重投影误差：

根据多视图几何得到点的三维坐标在当前帧的投影(运动方程)，与根据帧间估计得到上一帧的点在当前帧中该点的坐标(观测方程)。

投影点-匹配点=点的重投影误差

运动方程与观测方程的差为重投影误差

线的重投影误差：

三维直线两个端点投影到匹配到的特征线段所在直线的距离和

计算机视觉中的warp函数：

Warp这个操作本身可以理解为扭曲，变型，变换；其实就是一种数学运算，是一种统称，一般情况下paper里会定义怎么warp，不同建模warp function不同。

对于计算机几何视觉一般有

1）欧氏变换（SO3,SE3），自由度为3或者6，不变的是长度，夹角，体积；

2）相似变换，自由度为7，不变的是体积比；

3）仿射变换（Affine），自由度12，不变的是平行性，体积比；

4）射影变换，自由度15，相交性不变。

仿射变换参考
https://www.cnblogs.com/shine-lee/p/10950963.html

SLAM系统相关

李群李代数

引入李群李代数是用于简化优化求导问题。旋转矩阵不具有加法封闭性，同时具有正交约束。
给定一个旋转矩阵R，就能求得一个向量 ϕ [ a 1 , a 2 , a 3 ] T \phi[a_1,a_2,a_3]^T ϕ[a1​,a2​,a3​]T,他描述了R在局部时刻的导数关系，也就是对应的从李群 S O ( 3 ) SO(3) SO(3)到的李代数 s o ( 3 ) so(3) so(3)， R ( t ) R(t) R(t)的导数为 ϕ 0 ˆ R ( t ) \phi_0\^{}R(t) ϕ0​ˆR(t),其中 ϕ 0 ˆ \phi_0\^{} ϕ0​ˆ 为向量的反对称矩阵。
从 R R R到 ϕ \phi ϕ也是李群与李代数之间的指数/对数映射。
以上为旋转矩阵群SO3，下面是包含旋转和平移的李群SE3和李代数se3之间的关系

s e ( 3 ) = ξ = [ ρ ϕ ] se(3)=\xi = \begin{bmatrix} \rho\\ \phi \end{bmatrix} se(3)=ξ=[ρϕ​]

se(3)为六维向量，前三维描述平移，后三维为旋转
旋转矩阵群SO(3)上的指数映射也就是罗德里格斯公式，把李代数
相似变换群SE(3)的指数映射公式（与罗德里格斯公式的区别在于J）:

李代数求导

BCH公式

BCH主要用于两个矩阵指数之积（两个李代数指数映射乘积）的线性近似表达

当矩阵R2左乘一个微小变量，对应BCH中的第一种情况， J 1 ( ϕ ) − 1 ϕ 1 + ϕ 2 J_1(\phi)^{-1}\phi_1+\phi_2 J1​(ϕ)−1ϕ1​+ϕ2​可以近似的看做，在原来的李代数 ϕ 2 \phi_2 ϕ2​上加一项有关较小量的式子 J 1 ( ϕ ) − 1 ϕ 1 J_1(\phi)^{-1}\phi_1 J1​(ϕ)−1ϕ1​,第二种情况同理
，左扰动基础上对自变量取负号。

SO(3)上的李代数求导

左扰动求导相对于直接求导可以省去一个雅克比矩阵 J t J_t Jt​的计算

卡尔曼滤波过程：

https://www.cnblogs.com/geoffreyone/p/16161061.html

运动方程

观测方程
线性卡尔曼滤波主要是五个公式

状态预测

通过 k − 1 k-1 k−1次的后验 x k − 1 p x^p_{k-1} xk−1p​代入运动方程得到第k次的先验状态 x k c x_k^c xkc​

x k c = A k x k − 1 p + u k x_k^c=A_kx^p_{k-1}+u_k xkc​=Ak​xk−1p​+uk​

协方差预测

通过k-1次的后验协方差矩阵 P k − 1 p P_{k-1}^p Pk−1p​高斯分布的线性组合得到的第k次先验协方差矩阵 P k c P^c_k Pkc​

P k c = A k P k − 1 p A k T + R P^c_k=A_kP_{k-1}^pA^T_k+R Pkc​=Ak​Pk−1p​AkT​+R，其中R为高斯噪声矩阵

卡尔曼增益计算

通过第k次的先验协方差矩阵 P k c P^c_k Pkc​和观测方程的系数矩阵 C k T C^T_k CkT​就可以解出卡尔曼增益K
K = P k c C k T ( C k P k c C k T + Q k ) − 1 K=P^c_kC^T_k(C_kP^c_kC^T_k+Q_k)^{-1} K=Pkc​CkT​(Ck​Pkc​CkT​+Qk​)−1

状态更新

通过第k次的先验状态估计 x k c x^c_k xkc​和卡尔曼增益 K K K可以计算出第k次的后验状态估计 x k p x^p_k xkp​

x k p = x k c + K ( z k − C k x k c ) x^p_k=x^c_k+K(z_k-C_kx^c_k) xkp​=xkc​+K(zk​−Ck​xkc​)

协方差更新

通过第k次的先验协方差矩阵和卡尔曼增益K可以计算出第K次的后验协方差矩阵
P k p = ( I − K C k ) P k c P^p_k=(I-KC_k)P^c_k Pkp​=(I−KCk​)Pkc​

EKF扩展卡尔曼滤波

主要思路是在非线性函数的局部做泰勒展开，将非线性函数近似成线性函数

运动模型： f ( x k − 1 , v k , w k ) ≈ x k c + F k − 1 ( x k − 1 − x k − 1 p ) + w k ′ f(x_{k-1},v_k,w_k)\approx x^c_k+F_{k-1}(x_{k-1}-x^p_{k-1})+w'_k f(xk−1​,vk​,wk​)≈xkc​+Fk−1​(xk−1​−xk−1p​)+wk′​

观测模型： g ( x k , n k ) ≈ y c + G k ( x k − x k c ) + n k ′ g(x_k,n_k)\approx y^c+G_k(x_k-x^c_k)+n'_k g(xk​,nk​)≈yc+Gk​(xk​−xkc​)+nk′​

状态预测

通过 k − 1 k-1 k−1次的后验 x k − 1 p x^p_{k-1} xk−1p​代入运动方程得到第k次的先验状态 x k c x_k^c xkc​，

x k c = f ( x k − 1 p , v k , 0 ) x_k^c=f(x^p_{k-1},v_k,0) xkc​=f(xk−1p​,vk​,0)

协方差预测

通过k-1次的后验协方差矩阵 P k − 1 p P_{k-1}^p Pk−1p​高斯分布的线性组合得到的第k次先验协方差矩阵 P k c P^c_k Pkc​

P k c = F k − 1 P k − 1 p F k − 1 T + Q k ′ P^c_k=F_{k-1}P_{k-1}^pF^T_{k-1}+Q'_k Pkc​=Fk−1​Pk−1p​Fk−1T​+Qk′​，其中Q为高斯噪声矩阵

卡尔曼增益计算

通过第k次的先验协方差矩阵 P k c P^c_k Pkc​和观测方程的系数矩阵 C k T C^T_k CkT​就可以解出卡尔曼增益K
K = P k c G k T ( G k P k c G k T + R k ′ ) − 1 K=P^c_kG^T_k(G_kP^c_kG^T_k+R'_k)^{-1} K=Pkc​GkT​(Gk​Pkc​GkT​+Rk′​)−1

状态更新

通过第k次的先验状态估计 x k c x^c_k xkc​和卡尔曼增益 K K K可以计算出第k次的后验状态估计 x k p x^p_k xkp​

x k p = x k c + K ( y k − g ( x k c , 0 ) ) x^p_k=x^c_k+K(y_k-g(x_k^c,0)) xkp​=xkc​+K(yk​−g(xkc​,0))

协方差更新

通过第k次的先验协方差矩阵和卡尔曼增益K可以计算出第K次的后验协方差矩阵
P k p = ( I − K G k ) P k c P^p_k=(I-KG_k)P^c_k Pkp​=(I−KGk​)Pkc​

EKFSLAM相关

https://zhuanlan.zhihu.com/p/399365812

MSCKF

MSCKF全称Multi-State Constraint Kalman Filter（多状态约束下的Kalman滤波器），是一种基于滤波的VIO算法。

https://zhuanlan.zhihu.com/p/76341809

RANSAC

https://blog.csdn.net/leonardohaig/article/details/104570965

RANSAC算法基本思想：

1. 从数据集中随机选出一组局内点（其数目要保证能够求解出模型的所有参数），计算出一套模型参数。
2. 用得到的模型去测试其他所有的数据点，如果某点的误差在设定的误差阈值之内，就判定其为局内点，否则为局外点，只保留目前为止局内点数目最多的模型，将其记录为最佳模型。
3. 重复执行1,2步足够的次数（即达到预设的迭代次数）后，使用最佳模型对应的局内点来最终求解模型参数。
4. 最后可以通过估计局内点与模型的错误率来评估模型。

RANSAC中的内点和外点：

内点：符合模型的估计的点成为内点

外点：不符合模型估计的点成为外点

SLAM中旋转矩阵、旋转向量（轴角）、欧拉角、四元数的转换关系

旋转表示

https://blog.csdn.net/m0_46244540/article/details/104129405#t7

旋转矩阵

从世界坐标系到相机坐标系的坐标变换与逆变换

R 1 X + t 1 = Y R 2 Y + t 2 = X R 2 = R 1 T = R 1 − 1 , R 2 R 1 = I ( R 为正交矩阵 ) t 2 = X − R 2 Y = X − R 2 ( R 1 X + t 1 ) = X − R 2 R 1 X − R 2 t 1 t 2 = − R 2 t 1 R_1X+t_1=Y\\ R_2Y+t_2=X\\ R_2=R_1^T=R_1^{-1},R_2R_1=I (R为正交矩阵)\\ t_2=X-R_2Y=X-R_2(R_1X+t_1)=X-R_2R_1X-R_2t_1 \\t_2=-R_2t_1 R1​X+t1​=YR2​Y+t2​=XR2​=R1T​=R1−1​,R2​R1​=I(R为正交矩阵)t2​=X−R2​Y=X−R2​(R1​X+t1​)=X−R2​R1​X−R2​t1​t2​=−R2​t1​

//齐次变换矩阵T中取旋转矩阵R和平移向量t
//Rcw为世界坐标系到相机坐标系的旋转矩阵
const cv::Mat Rcw = mCurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
//从相机坐标系到世界坐标系的逆变换，由于旋转矩阵为正交矩阵
//转置即为逆矩阵
const cv::Mat Rwc = Rcw.t();
//tcw为世界坐标系到相机坐标系的平移向量
const cv::Mat tcw = mCurrentFrame.mTcw.rowRange(0,3).col(3);
//R1X+T1=Y
//R2Y+T2=X
//T2=X-R2Y=X-R2（R1X+T1）=X-R2R1X-R2T1
const cv::Mat twc = -Rcw.t()*tcw;
//旋转矩阵和平移向量t合成齐次变换矩阵T
cv::Mat Mod = cv::Mat::eye(4,4,CV_32F);//初始估计模型 欧式变换矩阵T 包含旋转矩阵和平移向量
// assign the result to current pose
Mod.at<float>(0,0) = R.at<double>(0,0); Mod.at<float>(0,1) = R.at<double>(0,1); Mod.at<float>(0,2) = R.at<double>(0,2); Mod.at<float>(0,3) = Tvec.at<double>(0,0);
Mod.at<float>(1,0) = R.at<double>(1,0); Mod.at<float>(1,1) = R.at<double>(1,1); Mod.at<float>(1,2) = R.at<double>(1,2); Mod.at<float>(1,3) = Tvec.at<double>(1,0);
Mod.at<float>(2,0) = R.at<double>(2,0); Mod.at<float>(2,1) = R.at<double>(2,1); Mod.at<float>(2,2) = R.at<double>(2,2); Mod.at<float>(2,3) = Tvec.at<double>(2,0);

1. SLAM中编程使用频繁，重点掌握
2. 旋转矩阵R具有正交性，R和R的转置RT的乘积是单位阵，行列式为1
3. 旋转矩阵R的逆矩阵表示一个和R相反的旋转
4. 旋转矩阵R通常和平移向量一起组成齐次变换矩阵T，描述欧式坐标变换，引入齐次坐标是为了可以方便的描述连续的欧氏变换
5. 冗余，9个元素表示三个自由度的旋转，比较冗余

优点：旋转轴可以是任意向量
缺点：旋转其实只需要知道一个向量+一个角度(共4个信息值)，但矩阵却用了16个元素(矩阵法消耗时间和内存)

旋转矩阵对三维点进行旋转：

P = [ x , y , z ] P ′ = R P + T P=[x,y,z] P'=RP+T P=[x,y,z]P′=RP+T，其中R为从 P P P点所在坐标系转化为 P ′ P' P′所在坐标系,利用旋转矩阵R做逆变换为： P = R − 1 ( P ′ − T ) P=R^{-1}(P'-T) P=R−1(P′−T)

相似变换sim(3)的变换和逆变换，在ORB-SLAM2回环检测部分用于相互投影匹配

Sim(3)相似变换为：

S = [ s R t O 1 ] S= \begin{bmatrix} sR&t\\O&1 \end{bmatrix} S=[sRO​t1​]

相似变换的逆矩阵为：

S − 1 = [ 1 s R T − 1 s R T t O 1 ] S^{-1}= \begin{bmatrix} \frac{1}{s}R^T&-\frac{1}{s}R^Tt\\O&1 \end{bmatrix} S−1=[s1​RTO​−s1​RTt1​]

代码示例：

cv::Mat sR12=s12*R12;
cv::Mat sR21=(1/s12)*R12.t();
cv::Mat t21 = -sR21*t12;

四元数

1. SLAM中编程使用频繁程度接近旋转矩阵

2. 四元数由一个实部三个虚部组成，是一种非常紧凑、没有奇异的表达方式（奇异矩阵不是满秩）

3. 编程时候很多坑，必须注意。首先，一定要注意四元素定义中实部虚部和打印系数的顺序不同，很容易出错！
   其次，单位四元素才能描述旋转，所以四元素使用前必须归一化：q.normalize()。

   

优点：

• 可以避免万向锁；
• 只需要一个4维的四元数就可以执行绕任意过原点的向量的旋转，方便快捷，在某些实现下比旋转矩阵效率更高；
• 四元数旋转可以提供平滑插值。

缺点：比欧拉旋转稍微复杂了一点，因为多了一个维度，理解更困难，不直观。

四元数对三维点进行旋转：

空间点 P = [ x , y , z ] P=[x,y,z] P=[x,y,z]，用虚四元数来表示为 P = [ 0 , x , y , z ] T P=[0,x,y,z]^T P=[0,x,y,z]T，旋转用一个单位四元数 q q q来表示，则旋转后的三维点 P ′ P' P′用四元数表示为：
P ′ = q P q − 1 = q P q ∗ P'=qPq^{-1}=qPq^{*} P′=qPq−1=qPq∗
四元数 P ′ P' P′的虚部取出也就是旋转后的三维坐标。其中 q ∗ q^* q∗表示 q q q的共轭四元数。
从四元数q=[s,v]^T到旋转矩阵的变换关系：
R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 R=vv^T+s^2I+2sv^\wedge+(v^\wedge)^2 R=vvT+s2I+2sv∧+(v∧)2

旋转向量

旋转向量定义：

            cv::Mat Rvec(3, 1, CV_64FC1);//旋转向量cv::Mat Tvec(3, 1, CV_64FC1);//平移向量cv::Mat R(3, 3, CV_64FC1);//旋转矩阵cv::Rodrigues(Rvec, R);//罗德里格斯公式可以将旋转向量转换成旋转矩阵//也可以矩阵转向量

1. 用一个旋转轴n和旋转角θ来描述一个旋转，所以也称轴角。不过很明显，因为旋转角度有一定的周期性（360°一圈），所以这种表达方式具有奇异性。
2. 从旋转向量到旋转矩阵的转换过程称为 罗德里格斯公式。
3. 旋转向量和旋转矩阵的转换关系，其实对应于李代数和李群的映射

欧拉角

优点

• 把一次旋转分解成3次绕不同坐标轴的旋转，比如航空领域经常使用的“偏航-俯仰-滚转”（yaw，pitch，roll）就是一种欧拉角。该表达方式最大的优势就是直观。

缺点：

• 欧拉角在SLAM中用的很少，原因是它的一个致命缺点：万向锁。也就是在俯仰角为±90°时，第一次和第3次旋转使用的是同一个坐标轴，会丢失一个自由度，引起奇异性。事实上，想要表达三维旋转，至少需要4个变量。

常用误差指标：

绝对轨迹误差（Absolute Trajectory Error,ATE),形式如下：

实际上是每个位姿李代数的均方根误差（RMSE）。这种误差可以刻画两条轨迹的旋转和平移误差。

绝对平移误差(Average Translations Error):

其中，Trans表示取括号内部变量的平移部分。

相对位姿误差(Relative Pose Error,RPE)定义如下：

同样可以只取平移部分：

超像素：

由一系列位置相邻且颜色、亮度、纹理等特征相似的像素点组成的小区域。

图像强度：

表示单通道图像像素的强度（值的大小）。

灰度图中，它是图像的灰度。

RGB颜色空间中，可以理解为它是R通道、G通道、B通道的像素灰度值，也就是RGB中的三个图像强度。

世界坐标系、物体坐标系和惯性坐标系之间的关系

世界坐标系->惯性坐标系：平移

惯性坐标系->物体坐标系：旋转

光流法：一种描述像素随着时间，在图像之间运动的方法。

稀疏光流：计算部分光流。

稠密光流：计算所有光流。

稀疏光流以LK光流法为代表的。

LK光流计算过程：

假设相机的图像是随时间变化的，图像看做是时间的函数：I(t)，在t时刻，位于(x,y)的像素，灰度为I(x,y,t)。

灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。

对于 t 时刻位于(x,y)位置的像素，设t+dt时刻，运动到(x+dx,y+dy)处，由于灰度不变：

对左边进行泰勒展开，保留一阶项，得：

由于灰度不变，下一个时刻的灰度等于之前的灰度，从而

两边除以dt，得

其中，dx/dt为像素在x轴上运动速度，而dy/dt为y轴速度，记为u,v。同时∂I/∂x为图像在该点处x方向的梯度，另一项是y方向的梯度。记为Ix，Iy。

图像灰度对时间的变化量记为It，写成矩阵形式为：

计算像素运动u,v，但是该式是带有两个变量的一次方程，无法解出，因此，必须引入额外的约束来计算 u,v。在 LK 光流中，我们假设某一个窗口内的像素具有相同的运动。 考虑一个大小为 w ×w 大小的窗口，它含有w²数量的像素。由于该窗口内像素具有同样的运动，因此我们共有个w²方程

用最小二乘法求解：

简化为：

最小二乘法求解：

本文链接：https://my.lmcjl.com/post/12123.html