3D相关HEF矩阵计算
F矩阵含义
- 相邻两帧一组对应像素点的约束关系
- 像素点在下一帧极线上的位置
E矩阵含义
本质矩阵是归一化平面下的基本矩阵的特殊形式
H矩阵意义
注：以上截图内容参考计算机视觉life
位姿求取
求取位姿的方法
- 对极约束：2D-2D，通过二维图像点的对应关系，恢复出在两帧之间摄像机的运动。
- PnP：3D-2D,求解3D到2D点对运动的方法。描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。
- ICP：3D-3D，配对好的3D点,已知世界参考系下的3D点和相机参考系下的3D点
PnP概念
如果场景的三维结构已知，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标即可
求解出相机坐标系与世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，该类求
解方法统称为N点透视位姿求解（Perspective-N-Point，PNP问题）。这里的控制点是指准确知道三
维空间坐标位置，同时也知道对应图像平面坐标的点。
已知条件：
- n3D参考点(3D reference points)坐标;
- 与这n个3D点对应的、投影在图像上的2D参考点(2D reference points)坐标;
- 相机的的内参K;
求解：
- 相机的位姿。
PnP应用场景
场景主要有两个
- 求解相机的位姿，一般应用于AR，人脸跟踪等；
通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机（相机坐标系）相对于真实物体（世界坐标系）的位姿
- 求取前一帧到当前帧的相机位姿变化，一般用于slam中；
通常输入的是上一帧中的3D点（在上一帧的相机坐标系下表示的点）和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换
Opencv中的函数
solvePnP
flags有如下的求解方法：
enum { SOLVEPNP_ITERATIVE = 0,
SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp
SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete
SOLVEPNP_DLS = 3, //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct
SOLVEPNP_UPNP = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive
SOLVEPNP_AP3P = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17
SOLVEPNP_MAX_COUNT //!< Used for count
};
提示
- SOLVEPNP_ITERATIVE，默认值，它通过迭代求出重投影误差最小的解作为问题的最优解。该方法
实质是迭代求出重投影误差最小的解，这个解显然不一定是正解。
使用范围：4对共面匹配点，小于4组点不准确
- SOLVEPNP_P3P，是使用非常经典的Gao的P3P问题求解算法。P3P算法要求输入的控制点个数只能是4对。3对点求出4组可能的解，通过对第4对点进行重投影，返回重投影误差最小的，确定最优解。可以使用任意4对匹配点求解，不要共面，特征点数量不为4时报错.
使用范围：4对不共面匹配点
- SOLVEPNP_EPNP，该方法使用EfficientPNP方法，求解问题EPnP使用大于等于3组点，是目前最有效的PnP求解方法。
使用范围：最少需要4对不共面的匹配点（对于共面的情况只需要3对）,点太少能运行，但不准确
注：方法 SOLVEPNP_DLS 和 SOLVEPNP_UPNP 不能使用，因为当前的实现是不稳定的，有时会给出完全错误的结果。如果传递这两个标志之一，则将使用 SOLVEPNP_EPNP 方法。
建议：小于4组匹配点时，用SOLVEPNP_ITERATIVE；在4组匹配点时，用SOLVEPNP_P3P；大于4组匹配点时，用SOLVEPNP_EPNP。
solvePnPRansac
solvePnP的一个缺点是对异常值不够鲁棒，当我们用相机定位真实世界的点，可能存在错配，对误
匹配进行Ransac过滤，RANSAC是“Random Sample Consensus（随机抽样一致）”的缩写
扩展-PnP问题求解原理
目前主要有直接线性变换（DLT），P3P，EPnP，UPnP以及非线性优化方法。
关于PnP的求解问题
把（3）式带入（1）式和（2）式，整理得：
我们可以看出，fx fy u0 v0是相机内参，上一节中已经求出，Xw Yw x y是一组3D/2D点的坐标，所以未知数有R11 R12 R13 R21 R22 R23 R31 R32 R33 T1 T2 T3一共12个，由于旋转矩阵是正交矩阵，每行每列都是单位向量且两两正交，所以R的自由度为3，秩也是3，比如知道R11 R12 R21就能求出剩下的Rxx。加上平移向量的3个未知数，一共6个未知数，而每一组2D/3D点提供的x y Xw Yw Zw可以确立两个方程，所以3组2D/3D点的坐标能确立6个方程从而解出6个未知数。故PnP需要知道至少3组2D/3D点。
DLT
通过2D-3D的关系直接构建方程，直接求解：
DLT主要是通过构建一个12维的增广矩阵（R|t），然后通过投影矩阵构建一个方程：
通过最后一行，消去s，最后构建一个12维的线性方程组，通过6对匹配点（一对点两个方程）来求解中间的矩阵。 最后将[R|t]左侧33矩阵块进行QR分解，用一个旋转矩阵去近似（将33矩阵空间投影到SE(3)流形上）。
优点：求解简单
缺点：直接将 T 矩阵看成了 12 个未知数，忽略了它们之间的联系。因为旋转矩阵 R ∈ SO(3)，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵。寻找一个最好的旋转矩阵对它进行近似
P3P（ICP:3D-3D）
采用一种变换的形式，把2D-3D匹配关系，变换成3D-3D点的匹配关系：
将世界坐标系下的ABC三点和图像坐标系下的abc三点匹配，其中AB，BC，AC的⻓度已知，
<a,b>，<b,c>，<a,c>也是已知，通过余弦定理得出方程组
类似于分解 E 的情况，该方程最多可能得到四个解，但我们可以用验证点来计算最可能的解，得到
A, B, C 在相机坐标系下的 3D 坐标。然后，根据 3D-3D 的点对，使用类似ICP的坐标系对，计算相
机的运动 R, t。
优点：需要的匹配点少
缺点：P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息；如果 3D 点
或 2D 点受噪声影响，或者存在误匹配，则算法失效。
可参考论文《CompleteSolution Classification for the Perspective-Three-Point Problem》
EPnP
将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和。
可参考论文《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》
