SLAM前端：PnP（一）DLS、P3P

PnP是一类问题的统称，是指通过多对点的3D位置及2D投影坐标，来估计相机位姿R、t。

场景一：视觉slam中在初始化后可以知道空间中一些点在世界坐标系下的坐标，在下一帧图像进行特征点匹配后，利用这些特征点的3D坐标及2D像素坐标，PnP可以计算出当前帧图像的R、t，即在世界坐标系下的R、t。

场景二：输入匹配特征点在上一帧的2D像素坐标、在上一帧相机坐标系下的3D坐标、在当前帧的2D像素坐标，求两帧之间的位姿R、t。

PnP是一大类问题，根据问题不同解法有P3P、DLS、EPnP、UPnP等。

1 P3P

对于场景一问题可以用P3P问题求解，需要三个点即可完成。需要条件：三个点的世界坐标、当前帧下的2D像素坐标。求解思路是首先求解三个点在相机坐标系下的坐标，再通过世界坐标系到像素坐标系的转换公式计算相机在世界坐标系下的R、t。

场景如上图所示，三个空间点ABC、图像投影点abc、相机光心O。计算过程如下：

首先根据余弦定理可得：

$\\ 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\\$

等式两边同时除以 $OC^2$ ，并令 $x = \frac{OA}{OC},y=\frac{OB}{OC}$ 得到：

$\\ x^2 + y^2-2xycos<a,b>=\frac{AB^2}{OC^2}\\ y^2 + 1-2ycos<c,b>=\frac{BC^2}{OC^2}\\x^2 + 1-2xcos<a,c>=\frac{AC^2}{OC^2} \\$

令 $u = \frac{AB^2}{OC^2},v = \frac{BC^2}{AB^2},w = \frac{AC^2}{AB^2}$ 得到：

$\\ x^2 + y^2-2xycos<a,b>=u\\ y^2 + 1-2ycos<c,b>=uv\\x^2 + 1-2xcos<a,c>=uw \\$

方程中已知量为 $u,v,w,cos<a,b>,cos<a,c>,cos<b,c>$ ，未知量为x,y，两个未知量将上述方程化简得到：

求解cos<a,b>:

得到角度为：

同理可以得到：cos<a,c>，cos<b,c>

2 DLS（直接线性变换）

设空间中某点P的世界坐标为 $[X,Y,Z,1]$ ，在图像 $I_1$ 中的2D坐标为 $[u,v,1]$ ，现在求解R、t。

首先定义曾广矩阵 $[R|t]$ ，由世界坐标系转像素坐标系有：

$s $\left[ \begin{matrix} u \\ v \\ 1 \end{matrix} \right] $= $\left[ \begin{matrix} 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{matrix} \right] $\cdot $\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right] $$

消去s得到：

$u = \frac{t_1 X + t_2Y+t_3Z+t_4}{t_9X+t_{10}Y+t_{11}Z+t_{12}},v = \frac{t_5 X + t_6Y+t_7Z+t_8}{t_9X+t_{10}Y+t_{11}Z+t_{12}}$

令等式右边为0得到：

$$\\$ [t_1 t_2 t_3 t_4]\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right] -[t_9 t_{10} t_{11} t_{12}]\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right]\cdot u =0 $\\$ $\\$[t_5 t_6 t_7 t_8]\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right] - [t_9 t_{10} t_{11} t_{12}]$\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right]\cdot v =0$

每个特征点提供两个方程，求解 $t_1-t_{12}$ 需要6个点。

SLAM前端：PnP（一）DLS、P3P相关推荐

视觉SLAM前端——PnP
如果我们已知多对匹配好的2D/3D特征点,我们可以用PnP方法去求得相机的运动.对于单目相机来说,我们可以通过利用对极约束或者单应性求得相机的运动,然后三角化获得3D点:对于双目可以通过视差恢复深度信 ...
激光SLAM 前端数据预处理--剔除坏点方法总结
激光SLAM 前端数据预处理--剔除坏点方法总结距离处理去除掉非常近的点 Code 测试结果分析去除掉非常近的点 Code 测试去除NaN的点 Code 反射率处理 Code 测试结果去 ...
PnP问题之P3P求解方式
PnP问题之P3P求解方式概念功能步骤链接补充概念 P3P方法是众多解决PnP问题中的一种方案,是通过已知的3对精准匹配的2D-3D点求解图像间相对的位姿变换的一种方法,所用到的信息较少. ...
视觉SLAM中PNP求解
PNP(Perspective-n-points)是SLAM中估计位姿的重要方法.已知条件为路标点在相机1中的相机坐标以及投影到相机2中的像素坐标,据此去估计相机1.相机2间的位姿.主要解法包括DLT ...
SLAM前端知识汇总
1.常用特征点汇总 1.1 Fast特征点 <SLAM14讲>中LK光流中用的特征点就是Fast特征点,然后对这些特征点进行光流追踪. 在8.3.2节的LK光流代码,第1帧提取的就是Fas ...
SLAM前端中的视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
SLAM前端 ---------特征提取之ORB（ORB与SIFT与SURF）
ORB 论文翻译: 一种特征匹配替代方法:对比SIFT或SURF 1.ORB特征简介 ORB是Oriented FAST and Rotated BRIEF(oFAST and rBRIEF)的简称 ...
视觉SLAM前端特征检测与跟踪的思考
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达就目前视觉SLAM的引用来区分,分为基于特征法的和直接法的视觉SL ...
视觉SLAM前端——ICP
当我们知道空间点在两个坐标系下的位置时,可以通过ICP来求解其相对位姿.ICP最初常用在激光SLAM中,因为激光可以获得3D位置,而不像视觉还需要经过相机的投影变换,但是从激光雷达获得的三维点云中,我 ...

SLAM前端：PnP（一）DLS、P3P

SLAM前端：PnP（一）DLS、P3P相关推荐

最新文章

热门文章