【视觉SLAM十四讲】第7讲 视觉里程计1

7.1 特征点法

前面提到过，SLAM中主要分为前端和后端，前端主要是根据相邻图像的信息，粗略估计相机的运动，从而给后端一个比较好的初始值。前端的实现，根据是否需要提取特征，分为特征点法和直接法。

对于特征点法，首先需要的就是定义什么是特征点。图像本身就是一个矩阵，如果直接从矩阵的层面考虑运动的估计，相当于用图像中全部的数据去估计，这样做的运算量必然是很大的。一般我们都会采用选代表的方法，也就是选取图像中比较有代表性的点。这些点在相机发生小幅度变化后会保持不变，所以可以在各个图像中找到相同的点，之后利用这些点，去讨论相机位姿估计问题以及点的定位问题。

特征是图像信息的另一种表达方式，一般数字图像在计算机中以灰度值矩阵的形式存在，而在视觉里程计中，我们希望特征点在相机运动之后能够保持稳定，灰度容易受到光照、形变等因素的影响，显然不够稳定。理想情况是场景和相机视角发生小幅度变化时，我们依然可以通过图像判断哪个地方是同一个点，这样仅仅依靠灰度值就不够了。
在视觉里程计中特征点就是图像中一些特别的地方，一般可以将角点、边缘和区块都看做一些有代表性的地方，不过判断同一个角点会相对简单，判断同一个边缘会因为局部相似而变得困难，判断区块则是最难的。因此图像中的角点、边缘相比于像素区块会更加特别，从而辨识度也就更高。因此在特征提取的时候，最直观的方法是提取不同图像间的角点，之后再确定它们之间的对应关系。

在进一步的研究中，研究人员定义了更多更加稳定的局部图像特征，他们都包括下面的特征：

特征点由两部分组成：关键点和描述子。关键点值的是该特征点在图像里的位置，有些特征点还包括朝向、大小等信息。描述子一般是一个向量，按照某种人为的方法去设定，用来描述周围的信息，一般设计描述子，都是按照“外观相似的特征应该有相似的描述子”原则进行的，因此只要描述子在向量空间的距离相近，就可以认为是同样的特征点。换句话说，关键点是具体的一个位置，在这个位置的基础上，用描述子来描述这个关键点周围的信息，比如我要描述一个人脸的特征，我不能只说一个位置，也不能只说一个样子，我必须是说明白哪里有个什么特征，鼻子下面有个痦子，鼻子下面就是一个关键点，有个痦子则是描述子。而我精心设计的描述子向量，必须能够体现我的特征，比如都是发黑，我要能够区别是痣还是皮肤癌，这样在实际比较的时候，某个向量离痣更近，那我就可以判断这个特征是个痣而不是皮肤癌。

在实际应用中，研究人员提出了很多类型的特征，其中SIFT是最经典的一种，它充分考虑了图像变换过程中出现的光照、尺度、旋转等变换，但是也带来了很大的计算量要求。ORB特征则是目前看来更具有代表性的特征，他改进了FAST特征子，不仅优化了方向问题，还采用二进制描述子，从而提高了计算速度。

下面简单介绍一下ORB特征。作为一个特征，ORB特征依然由关键点和描述子组成，它的关键点称为Oriented FAST，是一种改进的FAST角点。相较于原版的FAST角点，ORB中还计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变性。ORB的描述子称为BRIEF描述子，负责对前一步提取的特征点的周围图像区域进行描述。

FAST角点主要是检测局部像素灰度变化明显的地方，主要特点是速度快，这种特征的主要思想是如果一个像素与它邻域内的像素的差别较大，那就可能是角点。正因为这种相对简单的思想，FAST角点只需要比较像素亮度的大小，所以提取速度很快。它的检测过程如下：

可见这个提取过程中，主要是对一个点周围的16个像素点的灰度值进行比较，这种比较简单的过程决定了较快的提取速度。为了更加高效的提取，还可以在上述步骤中增加一个测试的步骤，可以看见在第四步中检测了是否有N个连续的点超过阈值，这里其实可以快速检测，具体操作为对于每个像素，检测待检测点中的1、5、9、13像素点的灰度值，只有当这四个点中有三个超过阈值，这个点才可能是一个角点，否则直接排除。这主要是对原步骤中的连续进行的快速判断，从而大大加快了检测的速度。

此外原始的FAST角点经常出现扎堆的现象，所以在第一遍检测之后，还需要使用非极大值抑制，这样一定的区域内就只保留响应最大值的角点。关于非极大值抑制，简称NMS算法，其思想是搜索局部最大值，抑制极大值，即在一个区域内存在多个极大值的时候，用区域内的最大值代表这个区域，而忽略非最大值。说白了就是在一个重合区域内只保留一个最大值，作为这个区域的代表。

FAST特征点的计算仅仅比较像素之间亮度的差异，速度很快，但是也存在一些问题。首先FAST特征点数量很大而且不确定，而我们一般希望提取固定数量的特征，对这一点不足，ORB中做出了改进，我们可以指定要提取的角点数目N，对原始的FAST特征点计算Harris响应值，选取响应值最大的N个作为最终的角点。其次，FAST角点没有方向信息，而且由于它在提取的过程中，使用固定半径为3的圆，所以还存在尺度问题。也就是说远处看着像角点的点，到近处看就不一定了，因为半径变化没有跟随尺度进行变化。针对这个不足，ORB中添加了尺度和旋转的描述，尺度不变性由构建图像金字塔并在每一层上检测角点来实现，而特征的旋转则是依靠灰度质心法来实现。
下面简单说一下灰度质心法，质心指的是以图像灰度值作为权重的中心，具体操作的步骤如下：

灰度质心法其实就是为图像块增加了一个灰度质心，利用灰度质心和几何中心的连线，从而额外构造出了一个方向信息。增加了尺度和旋转后，FAST角点的鲁棒性也大大提高，改进后的FAST角点就称为Oriented FAST。

在提取到Oriented FAST角点之后，就可以对每个点进行BRIEF描述子的计算。BRIEF是一种二进制描述子，也就是说它的描述向量由很多个01组成，这里的01代表关键点附近的两个像素的大小关系，大为1小为0。也就是说一对点就可以产生一位描述子，问题在于如何选择这点对，原始的论文中给出了很多种挑选方法，但大多都是按照某种概率，随机挑选的。
正是因为这种随机挑选，所以BRIEF的速度也是很快的，而且使用二进制描述子，所以存储上也很方便，从而适用于实时的图相匹配。而ORB在FAST的提取中计算了关键点的方向，所以可以利用方向信息，计算旋转后的特征，从而让ORB的描述子具有较好的旋转不变性。

现在描述子和特征点都已经介绍过了，下面就可以进行最关键的特征匹配了。特征匹配是SLAM中极为关键的一步，它主要是为了解决SLAM中的数据关联问题，也就是确定当前看到的路标与之前看到的路标之间的对应关系，通过比较两张图像或者图像与地图之间的描述子，就可以确定特征点之间的对应关系。需要注意，特征匹配是拿两个图的特征点去匹配，是点与点的匹配，这里容易和后面光流法的特征跟踪混淆。
由于图像特征的局部特征，匹配错误的情况经常出现，已经成为SLAM的一个主要瓶颈，部分原因是场景中含有很多的重复纹理，使得特征的描述十分困难。
对于正确情况的匹配，主要是进行两张图的特征点之间的比较，一个最简单的办法就是暴力匹配，就是每两个特征点之间都进行一次匹配，根据描述子确定一个距离，选择最近的作为一个匹配点，也就是说描述子距离实际上描述了两个特征之间的相似距离，距离越近，相似度就越高，这个距离的计算一般是使用汉明距离，也就是比较两个二进制字符串之间不同位数的个数。当然暴力匹配在数据量很大的情况会存在不足，此时可以使用更加快速的快速最近邻算法。

7.3 2D-2D:对极几何

首先扣一个帽子，对极几何主要是用于在2D坐标也就是像素坐标情况已知的情况下，利用图像之间的2D关系来估计相机的运动。

假设我们从两张图像中，得到了一对匹配好的特征点，我们现在可以利用这样几对匹配好的特征点，恢复出两帧之间摄像机的运动，以下图为例：

其中，I1I2是两个时刻得到的图像，P1P2分别为两个时刻图像上对应的特征点，O1O2为两个时刻的相机中心。这两个特征点显然是利用特征匹配得到的，在匹配成功的情况下，这两个点相当于是同一个三维点在两个位置上到成像平面的投影。所以真实物体点P和两个相机中心O1O2正好构成了一个三角形，或者说组成了一个平面，这个平面称为极平面，O1O2连线与成像平面I1I2交于e1e2，这两个点称为极点，O1O2称为基线，极平面与成像平面的交线称为极线。
明确了上面的一些基本名词，下面就可以延伸一些推理。从第一帧的角度来看，射线O1P1是某个像素在空间上可能出现的位置，这个不难理解，反正是投影到了p1，但是谁投影过来的不知道，必然在这条射线上。同时，如果不知道P的位置，那么从第二帧看的时候，P可能出现位置的投影就在l2上，这里本质上就是一个三角形关系，P可能出现的位置在P1-P这一段上，假设在这上面的任何一个位置，连接O2和假设的P，许多的线都会和I2有一个交点，这些交点都可以是p2，将这些被选的交点连起来，刚好就是射线l2。由于我们知道了p2的位置，所以可以恢复出P的空间位置，这一切都是特征匹配的结果，如果没有特征匹配，就没法确定p2在哪个位置，那种情况下就只能在极线上进行搜索，这部分搜索的知识会在13讲中介绍。

现在我们可以从代数的角度去对几何关系做一个描述，首先我们假设真实点P在相机坐标系下的坐标为：

根据针孔相机的投影模型，我们有下面的一个关系：

这里Z表示的是物体到相机中心的距离，左侧是一个归一化平面上的像素坐标，右边是相机内参矩阵和相机坐标系下的坐标，带入到我们上面的图，可以得到下面两个式子：

对这个式子，第一个其实就是针孔相机模型在这个场景下的具体化，s1是PO1的距离，p1是投影在I1上面的像素坐标，K是内参矩阵，P是在I1时刻相机坐标系下的坐标，而第二个式子只是做了个变换，这里用旋转R和平移t描述了变换到2位置后P的位置，即O2位置时点P在相机坐标系下的坐标，在这个位置下应该依然满足一个成像模型的等式，这个等式就是第二个式子。利用这种方式将两帧之间的旋转平移引入了进来。
在这两个式子的基础上，为了减少参数量，假设所有的投影结果都放在归一化平面上，即s1=s2=1，得到下面的式子：

上面左侧的式子，将P带入O1位置物体相机坐标系下的坐标，同理在O2位置也可以得到一个相同的式子，即：

将这两个式子带入上上式，化简后得到下面式子：

这个式子描述了同一个物体在两个相机坐标系下的位置变化关系，对这个式子左右同时左乘以t^再左乘x2T，得到下面的式子：

对于这个式子的左侧，结果是零，所以我们就得到了一个最简化的式子：

将p1p2的定义带入这个式子，也可以得到另一个形态的式子：

这两个式子都称为对极约束，这个式子表明O1PO2三点共面，这也是对极约束的几何意义。在对极约束中同时包括了平移和旋转，将其中的两个中间部分单独做一下定义，就得到了基础矩阵F和本质矩阵E，用这两个矩阵可以进一步简化对极约束：

可见对极约束给出了两个匹配点的空间位置关系，于是求相机位姿估计的问题就转换为了先根据对应特征点求出本质矩阵或者基础矩阵，之后根据这两个矩阵求旋转和平移。由于EF只差了一个相机内参，所以在实践过程中一般使用形式更为简单的E。

关于对极几何，结合几何意义的讲解参考下面链接：
https://cloud.tencent.com/developer/article/1436516

下面我们根据本质矩阵去反求旋转和平移。根据定义，本质矩阵是由旋转矩阵和平移向量组合出来的，从定义上来看，本质矩阵应该满足下面的要求：

根据这些要求，表明我们可以根据至少五对点来求解本质矩阵，由于本质矩阵的内在性质是一种非线性性质，所以在求解的时候会带来麻烦，所以一般使用八点法来求本质矩阵。

更新于2022年7月：
几天被师弟问到十四讲里面为什么说因为内在性质所以要使用八点法而不用五点法，查了半天，网上也没有靠谱的解释，这里补一下个人的新的理解。
首先本质矩阵的内在性质就是奇异值一定是一个固定的形式，其实个人感觉这个性质和解法的选择没有强制的限制。用八点法解本质矩阵，本质上源自于九点法，而九点法就是因为矩阵有九个未知数，解一个九个未知数的方程组，至少要九个量才能解出来，然而因为有尺度任意性，所以可以少一个参数，所以九点法变成了八点法。而我们知道本质矩阵的自由度是五，五自由度只能告诉我们，有某种方法可以用五对点就把这个矩阵解出来，但是这个方法并不是像八点法这样通过解方程的方法来实现的，必然要通过巧妙的计算来实现，而实现的方法书并没有给，所以就给人一种因为性质限制而选择八点法的感觉。
再回到问题本身，使用八点法不是说五点法不能用，主要还是因为五点法计算方法太复杂，不如八点法这种直白的方法来得快，而且对于实际场景来说，咋可能一个场景就个位数的匹配点，一般都是几十个几百个的匹配点，所以根本不需要在这三个点上纠结。除此之外，个人猜测也与关键矩阵用八点法有关，能统一用八点法，而且效果也不差，为什么要用两种方法而且其中一种还贼麻烦。

下面看一下八点法是如何工作的。拿一对匹配点，假设归一化坐标为x1=[u1,v1,1]T,x2=[u2,v2,1]，那么根据对极约束，应该有下面的式子：

将这个式子整理一下，换成向量相乘的形式，那么对极约束就可以写成一个与e相关的线性形式：

也就是说，一对特征点就可以写出这么一个式子，那么对其它的匹配点都做这样的处理，就可以得到下面的式子：

这个式子里，上标不表示次方，而表示第几对匹配点。现在修改形式之后，得到的就是一个线性方程组，线性方程组的系数矩阵是根据特征点位置构成的，而待求向量就是我们要求的本质矩阵的向量形式。现在问题转变为解这个线性方程组。只要八对匹配点满足矩阵的秩序等于8，就可以解出这个线性方程组。
解出本质矩阵后，我们要做的就是根据本质矩阵，恢复出平移旋转，这个过程需要使用奇异值分解，奇异值分解的过程如下：
根据本质矩阵的内在性质，对于一个本质矩阵，存在两个可能的R和t，也就是一共存在四个可能的解，对应下面的四种场景：

在这四种场景里面，只有一种能够让点在两个相机中都具有正的深度，因此我们要做的就是将任意一个点带入四种解中，检验深度的正负，就可以确定哪个是正确的解了。

除了基本矩阵和本质矩阵，我们还有一种矩阵称为单应矩阵（H），这个矩阵主要用来描述两个平面之间的映射关系，也就是许多的特征点都落在同一个平面上的情况。单应矩阵就是用来描述处在同一平面上的一些点，描述这两张图像之间俄变换关系。假设在I1和I1上有一对匹配好的特征点p1p2，这些特征点都落在同一个平面上，平面的方程为：

整理可得：

之后将这个式子带入对极约束中的第一步，可以得到下面的式子：

可见这个式子本质上也是一个描述图像坐标p1p2的变换，将中间红色框里面的部分记为H，这个矩阵就是单应矩阵，这个矩阵中包含了旋转、平移以及平面的参数，也是一个3×3的矩阵，求解的思路和求解本质矩阵类似，也是根据匹配点计算单应矩阵，之后分解得到旋转和平移。但是在实际处理中，由于我们引入了一个平面的限制，所以会有些不一样，我们按照求本质矩阵的过程计算，会有下面的式子：

在实际处理中，我们通常乘以一个非零因子让h9等于1，然后就可以根据第三行，去掉这个非零因子，得到下面的两个式子：

整理可得：

也就是说一对点就可以制造出两项约束，所以自由度为8的单应矩阵，我们可以用四对匹配点计算：

简单来说，就是当特征点集中在一个平面上时，由于平面的特殊性，会使得基础矩阵的自由度下降，即出现了退化，这些退化会让一些多余的自由度被噪声影响，进而扩大估计的误差，为了避免退化造成的影响，我们会使用基础矩阵和单应矩阵，选择重投影误差较小的那个作为最终估计的结果。

7.5 三角测量

三角测量，也叫做三角化，主要的用途在无法获得像素深度的单目SLAM中，估计地图中点的深度。具体来说，三角化是指通过在两处观测同一个点的夹角，来确定该点的距离。在SLAM中，就是说已知匹配点和两幅图像之间的变换（旋转平移），求特征点在真实空间中的深度。
三角化主要依靠下图的模型：

理论来说，我们在两次观测中都是对同一个点进行的观测，这就意味着理论上我们观测到的p应该会相交于同一点，也就是O1P1和O2P2的延长线理论上会交于一点，这个点P就是两个特征点在实际中的对应点。但是由于噪声的存在，一般这两条直线不会相交于一点，因此可以使用最小二乘法去求解。

按照对极几何的定义，假设x1x2为两个特征点的归一化坐标，他们会满足下面的式子：

这个式子其实还是利用了上面出现过的这个式子：

可以看出，上次用这个式子是利用点去求Rt，所以采用齐次坐标将s1s2给跳过了，而在这里用Rt是为了求距离，所以要保留s1和s2的原形式。现在我们已知Rt，要求s1s2，这两个式子可以分开求，如果要求s2，那么需要的就是左右同时乘以一个x1^，这个上箭头指的是将向量转换为反对称矩阵，得到下面的式子：

这个式子左侧为零，只剩下右侧，而右侧是一个关于s2的方程，可以利用这个方程直接求s2，有了s2，自然可以求出s1。于是我们就得到了两个帧下点的深度，也就是确定了真实点P的坐标。当然实际情况下，由于噪声的存在，上面这个式子不一定等于零，所以一般采用求最小二乘解，即寻找一个s1s2让总体最小，这样得到一个最优的距离。

这里顺便补充一下两个性质：尺度不确定性和纯旋转问题。
尺度不确定性是因为在上一节一开始的时候，有一步是采用了齐次坐标，这里相当于对位移进行了长度的归一化，这就导致了单目相机的尺度不确定性，我们对平移进行了归一化，相当于固定了尺度，我们不知道实际的尺度是多少，但是我们以为这时候单位是1，之后要通过一定的手段去恢复尺度。
纯旋转问题是一个很关键的问题，如果相机位置变换是只有旋转没有平移，那么对于后面的一切都是无法求解的，主要原因是，相机只进行旋转，就根本构建不了三角形，也就没有办法得到上面提到的全部公式和优化。

7.7 3D-2D:PnP

前面的对极几何，是根据两个2D的点，也就是照片上的像素坐标系去得到两个照片拍摄的位姿变换，引申出的三角化或者三角测量，是根据两个2D的点和位姿变换，去恢复深度。这两个方法用的都是对2D点的操作，这一节使用的PnP则不是这样，这个问题是求解3D到2D点对运动的方法，它描述了当我们知道n个3D空间点和他们的投影位置时，如何估计相机位姿。也就是说使用3D和2D的点来处理变换关系。
在2D-2D的对极几何中，需要至少8对点来估计位姿，而且还存在初始化、纯旋转和尺度的问题。在3D-2D中，如果一张特征点的3D位置已知，那么就只需要最少3对点就可以估计相机运动。

PNP问题的描述以及定义是相对简单的，他的目的就是求解3D-2D点对运动的方法。简单来说，就是在已知n个三维空间点坐标（相对于某个指定的坐标系A）及其二维投影位置的情况下，如何估计相机的位姿（即相机在坐标系A下的姿态）。举个例子，我们在一幅图像中，知道其中至少四个图像中确定的点在3D空间下的相对坐标位置，我们就可以估计出相机相对于这些点的姿态，或者说估计出这些3D点在相机坐标系下姿态。PnP实际上是一类问题的总称，n表示的就是使用点的数量，PnP实际上有很多种求解方法，比如使用3对点的P3P、直接线性变换，此外也可以使用非线性优化的方法，构建最小二乘问题来求最优解，也就是万金油的BA优化。

首先介绍最简单的直接线性变换，假设一个空间点P，这个P的齐次坐标为（X,Y,Z,1）T，在图像I1中投影的特征点为x1=（u1,v1,1)T，此时相机的位姿是不知道的，我们定义一个增广矩阵【R|t】，那么可以得到下面的式子：

利用最后一行将s去掉，可以得到两个约束：

可以看见这两个式子大量使用了增广矩阵中的一行的内容，为了表达简单，我们做下面的定义：

于是有：

现在向量t是未知量，是我们要求的量，不难看出，每一个特征点，通过2D和3D的限制条件，提供了对t的两个约束，假设一共有N个特征点，那么可以得到下面的线性方程组：

我们一开始假设的增广矩阵一共是12维，所以至少要12个匹配点，才可以实现上面这个线性方程组的求解，这种直接解的方法叫做直接线性变换。当匹配点大于12个的时候，可以使用奇异值分解的方法求最小二乘解。

直接线性变换需要12个也就是6对点去求解变换关系，而且求的是拍摄一张图片时候的位姿。下面介绍的P3P是另一种解PnP的方法，它使用3对匹配点，要求的数量较少，因此也相对简单一些。P3P主要是依赖于下图所示的模型：

P3P需要利用给的三个点的几何关系，它的输入是三对3D-2D的匹配点，3D点用大写字母表示，2D点用小写字母表示，此外还需要一对验证点，用于排除不可能的结果。
现在我们已知的是ABC的世界坐标系下的坐标，以及像素坐标系下的abc坐标。根据三角形的相似关系，我们可以写出下面的式子：

利用余弦定理，整理可以得到下面的三个式子：

对于这三个式子我们同时除以OC2，并且为了简单表达，将OA/OC和OB/OC分别记为x和y，那么可以化简为下面的式子：

继续简化，将v=AB2/OC2、uv=BC2/OC2、wv=AC2/OC2，那么可以得到下面的式子：

现在三个式子里面都有v，那么可以化简消掉一个式子，得到下面两个式子：

现在来看这两个式子，我们已知了2D点的位置，也就是说里面的cos都是可以求的，u=BC2/AB2,w=AC2/AB2，这两个值对应的是现实中的坐标，也可以直接计算出来，那么现在剩下的就只有xy是未知的，所以这实际上是一个关于xy的二元一次方程，只不过解起来很麻烦。这里解方程的任务交给数学院的学霸，我们只知道，这个方程组最多有四个解，这时候就需要用验证点，利用验证点来计算最有可能的解，从而得到ABC相机坐标系下的3D点。
现在，问题变为了已知不同时刻相机坐标系下的对应点，来求相机的位姿变换，这就将问题转换为了3D-3D的位姿估计问题，可以使用ICP之类的算法去求解具体的位姿变换。
所以说P3P实际上是利用三角形的相似性质，将问题转换为一个3D-3D的位姿估计问题，利用其他的方法去解具体的位姿。当然P3P也存在问题，首先P3P只用了3对点的信息，当给的点超过3对的时候，就很难利用其他的信息。另外如果点受到噪声的干扰，或者存在错误匹配，就很容易导致算法失效。

除了使用线性方法，非线性的BA优化也是很重要的方法。前面所介绍的两种方法，都是线性方法，他们往往是先求相机位置，之后再求空间点的位置，而非线性优化则是将他们全部看做优化变量，放在一起进行优化。
在PnP问题中，BA问题是一个最小化重投影误差的问题。我们假设n个三维空间点P和他们的投影p，现在我们要求相机的位姿，采用李代数的表示方法，根据前面的内容，可以写出下面的式子：

这个式子本质上还是下面这个式子的变形，将旋转和平移整合到了变换矩阵中，并且采用了变换向量转矩阵的表示方法。

现在由于相机位姿未知，而且存在观测点的噪声，上面的等式会存在一个误差，我们要做的就是将误差求和，构建最小二乘问题，然后寻找最好的相机位姿，将误差最小化，也就是下面的式子：

这个最小化问题的误差，是两个像素坐标系的差，可以看见减数位置的坐标是观测到投影位置的像素坐标，也就是实际观测的结果，而处在被减数位置的坐标，是我们根据变换矩阵，将3D点按照当前估计的位姿进行投影得到的位置坐标，将这两个坐标相减，得到的误差称为重投影误差，它衡量了我们变换矩阵的准确性，通过最小化这个式子，也就找到了让投影尽可能准确的变换矩阵。

就像上面这张图，我们通过特征匹配，知道p1p2都是三维空间点P在两个时刻的投影结果，但是我们不知道相机的位姿变换，所以我们假设一个位姿变换，结果投影到了p2^的位置，这个位置和p2存在不小的偏离，所以我们不断调整位姿，从而让这个距离变小，也就优化了位姿。

最后做一个小总结，PnP问题主要目标是求相机的位姿变换，也就是两帧图像之间的变换关系，这个变换关系包括一个旋转和一个平移。PnP问题有两种解决思路：线性和非线性。线性包括直接线性变换和P3P，非线性则对应BA优化。直接线性变换是利用许多点，通过变换关系，直接解出变换矩阵，而P3P则是利用三角形的相似关系，通过几何运算得到空间点在相机坐标系下的坐标，将问题转换为3D-3D的变换问题。BA优化则是将旋转平移放在一起考虑，通过最小化重投影误差来让相机位姿估计达到一个准确的程度。

7.9 3D-3D:ICP

各种点的匹配现在就剩下3D-3D的匹配了，这种情况下我们的目标就是一组匹配好的3D点，也就是两个相机坐标系的点，现在我们知道同一个真实空间的点在两个相机坐标系下的坐标，来求这两个相机坐标系的变换。在3D-3D的情景下，只考虑两组3D点的变换，所以和相机没有关系，也就不需要考虑相机模型。

这个问题最常用的方法就是迭代最近点问题，也就是ICP算法，关于ICP在之前整理过，这里直接放链接：
https://blog.csdn.net/weixin_43849505/article/details/121356403?spm=1001.2014.3001.5501

ICP有两种求解方法：利用线性代数的求解（奇异值分解）和利用非线性优化的方法。
前者在链接里整理过，主要是先将坐标换为去质心坐标，相当于移动到中心位置，之后优化计算旋转矩阵，进而计算出平移。后者则是使用迭代的方法去寻找最优值，需要使用李代数的扰动模型，不断迭代找到极小值，在唯一解的情况下这个最小值就是最优值。