第七讲视觉里程计1

喵喵不爱吃鱼

已于 2024-08-13 15:21:11 修改

阅读量113

点赞数 1

分类专栏：视觉SLAM十四讲文章标签：计算机视觉

于 2024-07-05 22:50:52 首次发布

本文链接：https://blog.csdn.net/weixin_42391513/article/details/140118740

版权

视觉SLAM十四讲专栏收录该内容

8 篇文章 0 订阅

订阅专栏

一个SLAM系统分为前端和后端，其中前端也称为视觉里程计。视觉里程计根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初始值。视觉里程计的算法主要分为两个大类：特征点法和直接法。本讲，将从介绍特征点入手，学习如何提取和匹配特征点，以及如何根据配对的特征点估计两帧之间的相机运动。

1、特征点

1.1 特征点

视觉里程计的核心是：如何根据图像估计相机运动。在视觉SLAM中，路标是指图像特征。在视觉里程计中，我们希望特征点在相机运动后保持稳定。
稳定的局部图像特征：SIFT、SURF、ORB，相对朴素的角点，这些人工设计的特征点拥有如下性质：
- 可重复性：相同特征可以在不同的图像中找到。
- 可区别性：不同的特征有不同的表达。
- 高效率：同一图像中，特征点数量应远小于像素数量。
- 本地性：特征仅与一小片图像区域有关。
特征点由关键点（Key-point）和描述子（Descriptor）两部分组成。
- 关键点：在图像中的位置，有的还有朝向和大小
- 描述子：通常是一个向量，描述该关键点周围像素的信息。按照“外观相似的特征应该有相似的描述子”的原则设计。
一些特征点：
- SIFT（尺度不变特征变换，Scale--Invariant Feature Transform）。最经典，充分考虑了图像变换中出现的光照、尺度、旋转等变化，但伴随的是极大的计算量。经过GPU加速后的SIFT，可以满足实时计算要求。
- FAST关键点数据计算特别快的特征点，但不具有方向性。
- ORB（Oriented FAST and Rotated BRIEF）特征是目前看来非常具有代表性的实时图像特征。改进了FAST检测子不具有方向性的问题，并采用速度极快的二进制描述子BRIEF（Binary Robust Independent Elementary BRIEF）。

1.2 ORB特征

ORB特征
- Oriented FAST关键点，相对于FAST，计算了特征点的主方向，为后续BRIEF描述子增加了旋转不变性。
- BRIEF描述子：对前一步提取的特征点的周围图像区域进行描述。ORB对BRIEF进行了改进，使用了先前计算的方向信息。
FAST关键点
- FAST是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的思想是：如果一个像素与邻域的像素亮度差别较大，那么更可能是角点。它的检测过程：
1. 在图像中选择像素p，假设它的亮度是 $I_p$ ；
2. 设定阈值T（例如， $I_p$ 的20%）；
3. 以像素p为中心，选取半径为3的圆上的16个像素点；
4. 如果选取的圆上有连续的N个点的亮度大于 $I_p + T$ 或者小于 $I_p - T$ ，那个像素p可以被认为是特征点（N通常选12，即FAST-12。其他常用的N取值为9、11）；
5. 循环以上四步，对每个像素执行相同的操作。
- FAST-12算法中，为了更高效，通过对每个像素检测邻域圆上第1、5、9、13个像素的亮度，只有当4个像素中有3个像素同时大于 $I_p + T$ 或者小于 $I_p - T$ ，当前像素才有可能是一个角点。否则直接排除。
- 另外，原始FAST角点容易出现扎堆现象，所以在第一遍检测后，还需要用极大值抑制（Non-maximal suppression），在一定区域内仅保留相应极大值的角点，避免角点扎堆。
- FAST特征点计算速度快，但存在重复性不高、分布不均匀的特点。另外FAST角点不具有方向信息，且其固定取半径为3的圆，存在尺度问题。
- 针对FAST角点不具有方向性和尺度的问题，ORB添加了尺度和旋转的描述。
1. 尺度不变性由构建图像金字塔，并在金字塔每一层上检测角点来实现。
2. 特征的旋转由灰度质心法实现。在一个小的图像块中，定义图像块的矩 $m_{pq}=\sum_{x,y\in B}x^py^qI(x,y)$ $p,q = \{0,1\}$ ；通过矩可以找到图像块的质心: $C = (\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$ ；连接图像块的几何中心与质心，得到一个方向向量OC，于是特征点的方向可以定义为： $\theta =\arctan (m_{01}/m_{10})$
- 通过以上方法，FAST角点便有了尺度和旋转的描述，大大提升了在不同图像之间表述的鲁棒性。所以在ORB中，这种改进后的FAST成为Oriented FAST。
BRIEF描述
- BRIEF 是一种二进制描述子，其描述向量由许多0和1组成，这里的0和1编码了关键点附近两个随机像素（比如p、q）的大小关系：p>q取1，反之取0。如果取了128个p、q，就得到128维由0、1组成的向量。
- 其特点速度快、存储方便，适用实时的图像匹配。
- ORB 在 FAST提取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转之后的“steer BRIEF”特征，使ORB的描述子具有较好的旋转不变性。

1.3 特征匹配

通过图像与图像之间或图像与地图之间的描述子进行准确匹配，可以为后续姿态估计、优化等工作减轻较大的负担。
最简单的特征匹配为暴力匹配，即对每个特征点与所有的测量描述子距离，取最近的一个作为匹配点。描述子距离表示了两个特征之间的相似程度，实际运用中可以取不同的距离度量范式。
- 浮点类型的描述子，使用欧氏距离
- 二进制的描述子，一般使用汉明距离（不同位数的个数）
当特征点数量很大时，暴力匹配运算量很大，快速近似最近邻算法（FLANN）更适合。

2、特征提取与匹配

2.1 OpenCV中的ORB特征

未筛选的匹配有大量的误匹配，筛选后，匹配数量减少了很多，但大多数匹配正确。这里，筛选的依据是汉明距离小于最小距离（最小距离与30取下限）的两倍，是一种工程上的经验方法，不一定有理论依据。

2.2 手写ORB特征

手写ORB代码主要有两步：ORB计算代码和匹配代码。

ORB计算中，用256位的二进制描述，则对应到8个32位的unsigned int数据，定义为：typedef vector<uint32_t> DescType。然后再结合Oriented FAST方式计算的角度信息计算描述子。整个过程通过比较旋转和平移后的两个点在图像上的灰度差异来构建ORB描述符的每一位，从而实现对图像局部特征的二进制编码。

DescType desc(8, 0);
for (int i = 0; i < 8; i++) {
  uint32_t d = 0;
  for (int k = 0; k < 32; k++) {
    int idx_pq = i * 32 + k;
    cv::Point2f p(ORB_pattern[idx_pq * 4], ORB_pattern[idx_pq * 4 + 1]);
    cv::Point2f q(ORB_pattern[idx_pq * 4 + 2], ORB_pattern[idx_pq * 4 + 3]);

    // rotate with theta
    cv::Point2f pp = cv::Point2f(cos_theta * p.x - sin_theta * p.y, sin_theta * p.x + cos_theta * p.y)
                     + kp.pt;
    cv::Point2f qq = cv::Point2f(cos_theta * q.x - sin_theta * q.y, sin_theta * q.x + cos_theta * q.y)
                     + kp.pt;
    if (img.at<uchar>(pp.y, pp.x) < img.at<uchar>(qq.y, qq.x)) {
      d |= 1 << k;
    }
  }
  desc[i] = d;
}

匹配代码中，使用了SSE指令集中的_mm_popcnt_u32函数计算一个unsigned int变量中1的个数，从而达到计算汉明距离的效果。

2.3 计算相机运动

有了匹配好的点对，就可以根据点对估计相机的运动。根据相机原理不同，估计方法也不同：

当相机为单目时，我们只知道2D的像素坐标，因此问题是根据两组2D点估计相机运动。该问题用对极几何解决。
当相机为双目、RGB-D时，或通过某些方法得到了距离信息，那么问题就是根据两组3D点估计相机运动。该问题用ICP解决。
当一组为2D，一组为3D（例如：帧和地图），也就是我们得到了一些3D点和它们在相机的投影位置，也能估计相机运动。该问题通过PnP求解。

3、2D-2D：对极几何

3.1 对极约束

几何关系：

P在两个图像的投影为 $p_1,p_2$
两个相机之间的变换为 $T_{12}$ ，（即 $p_1=T_{12}p_2$ ）
$O_1P$ 在第二个图像上投影为 $e_2p_2$ ，记为 $l_2$ ，称为基线，反之亦然。
$e_1,e_2$ 成为极点

实践中，已知两个图像点，怎样计算相机运动以及怎样确定地图点：

已知：通过特征匹配得到 $p_1,p_2$
求解： $T_{12}$ ，另外， $P,e_1,e_2$ 未知。

对极约束推导过程：

世界坐标： $P=[X,Y,Z]^T$
以第一张图像为参考，投影方程（单位像素）： $s_1p_1=KP, s_2p_2=K(RP+t))$ 。注： $s_2p_2=K(R_{21}P_1+t_{21})$
使用归一化坐标（去掉内参），上式左乘 $K^{-1}$ 取： $x_1=K^{-1}p_1, x_2=K^{-1}p_2$ ，代入上式得到： $x_1=P,x_2=RP+t$ ，进而得到齐次关系： $x_2=Rx_1+t$ （单位m）
两侧左乘t^： $t^{\wedge}x_2=t^{\wedge}Rx_1+t^{\wedge}t$ ，即 $t^{\wedge}x_2=t^{\wedge}Rx_1$
再进一步左乘： $x_2^Tt^{\wedge}x_2=x_2^Tt^{\wedge}Rx_1$
最终得到对极约束： $x_2^Tt^{\wedge}Rx_1=0$
带内参的对极约束形式： $p_2^TK^{-T}t^{\wedge}RK^{-1}p_1=0$

对极约束的几何意义是O1,O2,P共面的关系。对极约束中同时包含了旋转和平移。我们把中间部分记作两个矩阵：本质矩阵E（Essential矩阵）和基础矩阵F（Fundamental矩阵）。

$E=t^{\wedge}R,F=K^{-1}EK^{-1},x_2^TEx_1=p_2^TFp_1=0$
在内参已知的情况下，可使用E。

通过对极约束，相机位姿估计变为两步：

通过匹配点计算E（八点法）；
由E恢复R,t（SVD分解，即奇异值分解）

3.2 本质矩阵

对极约束的性质：

对极约束是等式为零的约束，所以乘上任意非零常数依然满足。这称作E在不同尺度下等价的。
$E=t^{\wedge}R$ ，可以证明，本质矩阵E的奇异值必定是 $[\sigma ,\sigma ,0]^T$ 的形式，这称为本质矩阵的内在性质。
E共有5个自由度（t和R各3个自由度，由于尺度等价，因此可去掉1个自由度）
- 最少用5对点，可以求出E，但是五点法需用到非线性的运算，非常麻烦。
- 当成普通矩阵的话，有8个自由度（3*3矩阵共9个自由度，去掉1个）。八点法只利用了E的线性性质，可在线性代数框架下求解。

八点法求E

将E看成3乘3的矩阵，去掉因子剩8个自由度
一对匹配点带来的约束： $(u_1,v_1,1)\begin{pmatrix} e_1 & e_2&e_3 \\ e_4 & e_5 & e_6\\ e_7& e_8& e_9 \end{pmatrix}\begin{pmatrix} u_2\\ v_2\\ 1 \end{pmatrix}=0$
向量形式：将矩阵E展开，写成向量 $\boldsymbol{e} = [e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9]^T$ ，则对极约束可写成： $[u_2u_1,u_2v_1,u_2,v_2u_1,v_2u_1,v_2v_1,v_2,u_1,v_1,1]\cdot \boldsymbol{e}=0$
八点对构成方程组。其中系数矩阵8×9，由特征点位置构成l

从E计算R,t：奇异值分解

从E分解得到t,R时，一共存在4个可能得解，只有一个解中P在两个相机的深度都具有正深度。

八点法的性质：

单目SLAM初始化：单目视觉中，对两张图像的t归一化相当于固定了尺度，以这时的t为单位1，计算相机运动和特征点的3D位置，被成为单目SLAM的初始化。初始化之后，就可以用3D-2D计算相机运动了。初始化的两张图像必须有一定程度的平移。
尺度不确定性：实际运用中，会将t归一化（模长为1），或归一化特征点的平均深度
纯旋转问题：t=0时无法求解，因此必须带平移。
多于8点，最小二乘（ $Ae=0$ ， $\mathop{min}\limits_{e}||Ae||_2^2=\mathop{min}\limits_{e}e^TA^TAe$ ）或RANSAC

3.3 单应矩阵（Homography）

单应矩阵描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上，则可以通过单应性进行运动估计。这种情况再无人机携带的俯视相机或扫地机携带的顶视相机中比较场景。

假设特征点落在平面P上，设平面满足方程： $n^TP+d=0$ ，整理得 $-\frac{n^TP}{d}=1$
代入 $p_2\simeq K(RP+t)$ ，整理得 $p_2\simeq K(R-\frac{tn^T}{d})K^{-1}p_1$
单应矩阵H： $H=K(R-\frac{tn^T}{d})K^{-1}$ ，于是 $p_2\simeq Hp_1$
H的定义与旋转、平移及平面参数有关，与基础矩阵F类似，也是一个3×3的矩阵。
位姿估计求解：根据匹配点计算H，然后将H分解计算旋转和平移。

根据匹配点计算H：

一组匹配点对可以构造两项约束（实际三项，但因为线性想噶，只取前两个），于是自由度8的单应矩阵可以通过4对匹配特征点算出（在非退化的情况下，也就是这些特征点不能有三点共线的情况）。

单应矩阵的意义：

在SLAM中有重要的意义。当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，就出现退化。通常会同时估计基础矩阵F和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计矩阵。

3.4 对极几何总结

2D-2D情况下，只知道图像坐标之间的对应关系：
- 当特征点在平面上时（例如俯视、仰视），使用H恢复R,t
- 否则，使用E或F恢复R,t
求得R,t后，利用三角化计算特征点的3D位置（深度）

4、实践：对极约束求解相机运动

略

5、三角测量

通过上文已知运动R,t，求解特征点的3D位置。在单目SLAM中，仅通过单张图像无法获得像素的深度信息，需要通过三角测量（或三角化）的方法估计地图点的深度。

三角测量：通过不同位置对同一个路标点进行观察，从观察到的位置推断路标点的距离。

几何关系： $s_2x_2=s_1Rx_1+t$ 。其中 $x_1,x_2$ 为两个特征点的归一化坐标，已知R,t，求解两个特征点的深度s1,s2.

求解方法：

直接求解：若求s1，两侧左乘 $x_2^{\wedge}$ ，得到 $s_2x_2^{\wedge}x_2=0=s_1x_2^{\wedge}Rx_1+x_2^{\wedge}t$ ，进而再求得s2
更常见的方法：求最小二乘解： $[x_1,-Rx_2]\begin{bmatrix} s_1\\ s_2 \end{bmatrix}=t$

6、实践：三角测量

调研OpenCV提供的cv::triangulatePoints，它是一个Nx4的矩阵，包含了在第一个相机坐标系下恢复出的三维点的齐次坐标。每个点由四个元素组成：[X, Y, Z, W]，其中X、Y、Z是三维空间中的坐标，W是一个尺度因子。由于这些点是齐次坐标，所以实际的三维坐标应该是[X/W, Y/W, Z/W]。如果W接近零，那么该点可能是不稳定的或不可靠的。

当平移很小时，像素上的不确定性将导致较大的深度不确定性。
平移较大时，在同样的相机分辨率下，三角化测量将更精确。
要提高三角化的精度：
- 一种是提高特征点的提取精度，也就是图像分辨率，但会导致图像变大，增加计算成本；
- 一种是使平移变大，但会导致图像外观有明显变化。
因此，增大平移，可能导致匹配失效；而平移太小，则三角化精度不够。——这就是三角化的矛盾，这个问题成为“视差”（parallax）。
在单目视觉中，由于单目图像没有深度信息，我们要等待特征点被追踪几帧后，产生足够的视角，再用三角化来确定新增特征点的深度值。这有时也被称为“延迟三角化”。

7、3D-2D：PnP

PnP（Perspective-n-Point）是求解3D到2D点对运动的方法。描述了已知n个3D空间点及其投影位置时，如何估计相机的位姿。

解法有两类：代数法/优化法

代数解法：不够鲁棒
- 直接线性变换（DLT）
- 3对点估计位姿的P3P
- EPnP（Efficient PnP）、UPnP
非线性优化：构建最小二乘问题并迭代求解
- 光束平差法（Bundle Ajustment, BA）

7.1 直接线性变换（DLT）

已知一组3D点坐标，以及它们在某个相机中的投影位置，求该相机的位姿。

已知某空间点P的齐次坐标： $P=(X,Y,Z,1)^T$ ，在图像中，投影到特征点 $x_1=(u_1,v_1,1)$ （以归一化平面齐次坐标表示），求解R,t。

求解过程：

定义增广矩阵[R|t]为3×4的矩阵，保护旋转和平移信息。 $s\begin{pmatrix} u_1\\ v_1\\ 1 \end{pmatrix} = \begin{pmatrix} 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{pmatrix}\begin{pmatrix} X\\ Y\\ Z\\ 1 \end{pmatrix}$
为简化，定义T的行向量： $\boldsymbol{t_1}=(t_1,t_2,t_3,t_4)^T,\boldsymbol{t_2}=(t_5,t_6,t_7,t_8)^T,\boldsymbol{t_3}=(t_9,t_{10},t_{11},t_{12})^T$
用最后一行将s消掉，每个特征点可以得到两个关于 $\boldsymbol{t}$ 的约束： $\boldsymbol{t_1}^T\boldsymbol{P}-\boldsymbol{t_3^TP}u_1=0$ ， $\boldsymbol{t_2}^T\boldsymbol{P}-\boldsymbol{t_3^TP}v_1=0$
其中 $\boldsymbol{t}$ 是待求解的变量，可列出线性方程组。 $\boldsymbol{t}$ 一共12维，因此最少通过6对匹配点即可实现矩阵T的线性求解，该方法成为DLT。当匹配点大于6对时，可使用SVD等方法对超定方程求最小二乘解。
DLT求解时，将T矩阵看成了12个未知数，忽略了它们的关系。因为旋转矩阵 $R\in SO(3)$ ，因此DLT求出的解不一定满足该约束。可以通过QR分解完成。

7.2 P3P

P3P 输入为3对3D-2D匹配点，以及一对验证点以从可能得解中选出正确的一个。原理是：利用三角形形似性质，求解投影点a,b,c在相机坐标系下的3D坐标，最后把问题转换成一个3D到3D的位姿估计问题。

已知A、B、C的世界坐标系中的坐标，以及对应在相机成像平面上的投影a、b、c。

根据三角形的余弦定理，有：
- $OA^2+OB^2-2OA\cdot OB\cdot cos<a,b> = AB^2$
- $OB^2+OC^2-2OB\cdot OC\cdot cos<b,c> = BC^2$
- $OA^2+OC^2-2OA\cdot OC\cdot cos<a,c> = AC^2$
将上式除以，记：，，另外消掉v，代入后两个式子，得到：
- $(1-u)y^2-ux^2-cos<b,c>y+2uxycos<a,b>+1=0$
- $(1-v)x^2-wy^2-cos<a,c>x+2wxycos<a,b>+1=0$
上式中，x,y未知，因此该方程组是关于x,y的一个二元二次方程，通过吴消元法，得到A、B、C在相机坐标系下的3D坐标。
然后，根据3D-3D的点对，计算相机的运动R,t

7.3 最小化重投影误差求解PnP

前面说的线性方法，往往是先求相机位姿，再空间点位置。而非线性优化是把他们都看成优化变量。这一类把相机和三维点放在一起进行最小化的问题，统称为Bundle Ajustment。

考虑n个三维空间点P及其投影p，我们希望计算相机的位姿R,t，它的李群表示为T。像素位置与空间点位置关系： $s_i\begin{bmatrix} u_i\\ v_i\\ 1 \end{bmatrix}=\boldsymbol{KT}\begin{bmatrix} X_i\\ Y_i\\ Z_i\\ 1 \end{bmatrix}$ ，写成矩阵： $s_i\boldsymbol{u}_i=\boldsymbol{KTP}_i$ .
该等式存在误差，将误差求和，构建最小二乘问题，寻找最好的相机位姿，使误差最小化： $\boldsymbol{T}^*=arg \mathop{min}\limits_{T}\frac{1}{2}\sum_{n}^{i=1}||\boldsymbol{u}_i-\frac{1}{s_i}\boldsymbol{KTP}_i||_2^2$
该问题的误差项，是将3D点的投影位置与观测位置作差，所以称为重投影误差。在初始值R,t中，P的投影 $\hat{p_2}$ 与实际的 $p_2$ 之间有一定距离。于是需要调整相机的位姿，使得这个距离变小。
考虑单个投影点误差： $\boldsymbol{e}_i=\boldsymbol{u}_i-\frac{1}{s_i}\boldsymbol{Kexp(\xi ^\wedge)P}_i$
$\boldsymbol{e(x+\Delta x)}\approx \boldsymbol{e(x)}+\boldsymbol{J^T\Delta x}$ 。其中 $\boldsymbol{e}$ 是像素坐标误差（2维）， $\boldsymbol{x}$ 为相机位姿（6维）时， $\boldsymbol{J^T}$ 将是一个2×6的矩阵。 $\boldsymbol{J^T}$ 的推导优先考虑解析导数。
雅可比推导：链式法则得到 $\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi} }=\lim_{\delta \boldsymbol{\xi}->0}\frac{ \boldsymbol{e}(\delta \boldsymbol{\xi}\bigoplus \boldsymbol{\xi})}{\delta \boldsymbol{\xi}}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P'}}\frac{\partial \boldsymbol{P'}}{\partial \delta \boldsymbol{\xi}}$ ,P'是P在相机坐标系下的坐标。两项求导结果相乘。推导出的雅可比矩阵描述了重投影误差关于相机位姿李代数的一节变化关系。
除了位姿优化，还希望优化特征点的空间位置。因此需要讨论 $\boldsymbol{e}$ 关于空间点 $\boldsymbol{P}$ 的导数。 $\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P'}}\frac{\partial \boldsymbol{P'}}{\partial \boldsymbol{P}}$
由此，我们推导出观测相机方程关于相机位姿与特征点的两个导数矩阵。它们在优化过程中提供重要的梯度方向，指导优化的迭代。

8、实践：求解PnP

使用EPnP、手写高斯牛顿法BA优化、使用g2o进行BA优化。

g2o图优化中，节点和边的选择：

节点：第二个相机的位姿节点 $\boldsymbol{T}\in SE(3)$
边：每个3D点在第二个相机中的投影，以观测方程来描述： $z_j=h(T,P_j)$

9、3D-3D：ICP

在视觉中，ICP（迭代最近邻，Iterative Closest Point）指代匹配好的两组点间的运动估计问题。

$\forall i,\boldsymbol{p}_i=\boldsymbol{R}\boldsymbol{p}_i'+\boldsymbol{t}$

求解方式分为两种：线性代数求解（SVD），以及利用非线性优化方式求解（类似BA）。

9.1 SVD

定义第i对点的误差项： $e_i=p_i-(Rp_i'+t)$
构建最小二乘问题，求使误差平方和达到极小的R,t： $\mathop{min}\limits_{R,t}\frac{1}{2}\sum_{i=1}^{n}||p_i-(Rp_i'+t)||_2^2$
定义两组点的质心： $p=\frac{1}{n}\sum_{i=1}^{n}(p_i),p'=\frac{1}{n}\sum_{i=1}^{n}(p_i'),$
代入优化目标函数，简化得到： $\mathop{min}\limits_{R,t}J=\frac{1}{2}\sum_{i=1}^{n}||p_i-p-R(p_i'-p')||^2+||p-Rp'-t||^2$
优化目标函数，左边只与R有关，右边既有R又有t，但只与质心有关。只要获得R，令第二项为0就能得到t。
- 计算两组点的质心位置p,p'，然后计算每个点的去质心坐标： $q_i=p_i-p,q_i'=p_i'-p'$
- 根据以下优化问题计算旋转矩阵：
  - 展开关于R的误差项 $\frac{1}{2}\sum_{i=1}^{n}||q_i-Rq_i'||^2=\frac{1}{2}\sum_{i=1}^{n}(q_i'^Tq_i+q_i^TR^TRq_i'-2q_i^TRq_i')$
  - 第一、二项与R无关，实际优化目标函数： $\sum_{i=1}^{n}-q_i^TRq_i'=\sum_{i=1}^{n}-tr(Rq_i'q_i^T)=-tr(R\sum_{i=1}^{n}q_i'q_i^T)$
  - 定义 $W=\sum_{i=1}^{n}q_i'q_i^T$ ，对W进行SVD分解，得 $W=U\sum V^T$ 。其中 $\sum$ 为W的奇异值组成的对角矩阵，对角线元素从大到小排列，而U和V为对角矩阵。当W满秩时， $R=UV^T$
- 根据R计算t， $t^*=p-Rp'$

9.2 非线性优化方法

使用非线性优化方法，以迭代的方式去找最优值。以李代数表达位姿，目标函数可写成： $\mathop{min}\limits_{\xi }\frac{1}{2}\sum_{i=1}^{n}||p_i-exp(\xi ^\wedge)p_i'||_2^2$ 。

单个误差项关于位姿的导数，使用李代数扰动模型： $\frac{\partial e}{\partial \delta \xi}=-(exp(\xi^\wedge)p_i')^{\bigodot}$
在非线性优化中只需不断迭代，就能找到极小值。而且，在唯一解的情况下，只要能找到极小值解，这个解就是全局最优值。这就意味着ICP求解可以任意选的初始值，这是已匹配点时求解ICP的一大好处。

在某些场合下，例如RGB-D SLAM中，一个像素的深度数据可能有，也可能测不到，所以可以混合着使用PnP和ICP优化：对于深度已知的特征点，建模它们的3D-3D误差；对于深度未知的特征点，则建模3D-2D的重投影误差。于是，可以将所有误差放在同一个问题考虑，求解更方便。

11、一些定义

图像块的矩通常定义为像素值与坐标的乘积，然后对整个图像区域进行累加。
代表向量t的反对称矩阵。^符号，将一个向量变成了反对称矩阵。同理，对于任意反对称矩阵，我们也能找到唯一与之对应的向量，这个运输用符号表达。《SLAM十四讲》中P75
- $a^\wedge=A=\begin{bmatrix} 0 & -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0 \end{bmatrix}$ , $A^\vee=a$
快速最近邻搜索库（FLANN）：Fast Library for Approximate Nearest Neighbors。是开源的C++库，用于高效地查找数据中的最近邻。FLANN包含了一组算法和数据结构，可以在高维空间中快速找到最相似的数据点。
随机抽样一致性（RANSAC）：Random Sample Consensus
基本RANSAC的原理_ransac原理图-CSDN博客
单目SLAM的尺度不确定性：将轨迹和地图同时缩放若干倍，得到的观测值是不变的。
在三维空间中，我们可以使用向量的反对称矩阵来表示该向量与其他向量的叉积运算。
反对称矩阵：主对角线的元素都是0，且位于主对角线两侧对称的元素反号，且满足 $A^T=-A$ 。若A为反对称矩阵，则 $A'$ 与 $\lambda A$ 都是反对称矩阵，且 $x^TAx=0$ ；若A和B都是反对称矩阵，则A+B与A-B都是反对称矩阵。
SVD（singular value decomposition，奇异值分解），对于任意m行n列的矩阵A，奇异值分解的综合理解：
- 正交矩阵U：m行m列，该矩阵的每一个列向量都是 $AA^T$ 的特征向量
- 正交矩阵V：n行n列，该矩阵的每一个列向量都是 $A^TA$ 的特征向量
- 对角阵 $\sum$ ：m行n列，将 $AA^T$ 或 $A^TA$ 的特征值开根号，得到的就是该矩阵的主对角阵线上的元素，也可以看成矩阵A的奇异值。
A为n阶矩阵，若A的秩为n，则A为满秩矩阵。行向量、列向量线性无关的矩阵。
- 可逆性：满秩矩阵是可逆矩阵（非奇异矩阵），即存在逆矩阵使得A与其逆矩阵的乘积为单位矩阵。
- 线性无关：满秩矩阵的行向量与列向量都线性无关
- 秩的性质：矩阵的秩是其行向量或列向量中线性无关向量的最大个数。
A为n阶矩阵。若 $|A|\neq 0$ ，则A为非奇异矩阵。
对角矩阵：除了主对角线上的元素可能不为0外，其余元素都为0.
正交矩阵：其转置等于其逆的矩阵。
- 正交矩阵的每个行向量和列向量都是单位向量，并且这些向量之间两两正交。 $A_i\cdot A_j^T=0$ （内积为0，表示正交），且 $A_i\cdot A_i^T=1$ （自内积为1，表示单位向量）
- 正交矩阵乘以向量后，保持向量的长度和向量之间的角度不变。
- 正交矩阵的行列式的绝对值为1.
- 正交矩阵的转置矩阵也是正交矩阵。