位姿估计之EPnP算法学习笔记

首先给出<视觉slam十四讲>中是如何定义PnP:
PnP(Perspective-n-point )是求解3D到2D点对运动的方法。它描述了当我们知道n个3D空间点以及它们的投影位置时,如何估计相机所在的位姿。

**这里有两个概念比较重要:

1.3D空间点:指的是物体在世界坐标系下的坐标。
2.投影位置: 指的是物体在图像坐标系下的坐标。**

举一个例子:在空间中有一个立方体(8个顶点),这8个点在图像上分别对应着一个投影点。

那么pnp理解起来就比较容易了:
用自己的话说就是:已知物体在世界坐标系下N个点的世界坐标以及这N个坐标在图像上的投影点的坐标,来计算相机位姿的过程就叫做pnp求解。

已知量:N个点的世界坐标、图像坐标、摄像机内参矩阵K
求解量:相机位姿
求解方法:最小化重投影误差
Pnp的求解方法有很多:P3P, DLT, EPnp等

DLT:直接构建一个12个未知数的[R|t]增广矩阵(先不考虑旋转矩阵的自由度只有3),取六个点对,去求解12个未知数(每一个3D点到归一化平面的映射给出两个约束),最后将[R|t]左侧33矩阵块进行QR分解,用一个旋转矩阵去近似(将33矩阵空间投影到SE(3)流形上)。

P3P:P3P方法是通过3对3D/2D匹配点,求解出四种可能的姿态,在OpenCV calib3d模块中有实现,但是对于相机远离3D平面(low parallax)或者视角垂直于3D平面的情况下效果不佳。论文Complete Solution Classification for the Perspective-Three-Point Problem中提到了一种改进的方法,可以消除这种退化的情况。

EPnP:需要N对3D-2D匹配点,是目前最有效的PnP求解方法。
解决思路:通常选取世界坐标下的四个控制点坐标为Cw=[0,0,0,1]T, [1,0,0,1]T,[0,1,0,1]T,[0,0,1,1]T;通过n个3D点在相机平面的投影关系,以及与这四个控制点的权重关系,构建一个12*12方阵,求得其零空间特征向量,可以得到虚拟控制点的相机平面坐标,然后使用POSIT算法即可求出相机位姿。通常在用EPnP求得四对点下的封闭解后,可以将该解作为非线性优化的初值,优化提高精度

EPnP算法流程:

位姿估计之EPnP算法学习笔记_第1张图片
这里先解释一下下面符号的意义:
在这里插入图片描述
i=1,2,…,n 表示的是3D参考点在世界坐标系下的坐标;
在这里插入图片描述
i=1,2,…,n 表示的是3D参考点投影点的坐标;
在这里插入图片描述
j=1,2,3,4 表示的是4个控制点在世界坐标系下的坐标;

在这里插入图片描述
j=1,2,3,4 表示的是4个控制点投影点的坐标;

那么对于世界坐标系中的任意3D点都可用四个控制点表示:
在这里插入图片描述
其中:是齐次barycentric坐标。
控制点一旦确定,且不共面的话,那么,j=1,2,3,4就是唯一确定的。

同样的对于相机坐标系也有这样的关系:
在这里插入图片描述
假设摄像头的外参为:【R, t】,那么虚拟控制点和之间存在:
在这里插入图片描述
由于EPnp算法考虑到将参考点坐标表示为控制点坐标的加权和,那么就有:
在这里插入图片描述
又因为
,带入上式中就有:在这里插入图片描述
在这里插入图片描述
化简得到:
位姿估计之EPnP算法学习笔记_第2张图片
那么就可以得出,任意点P的相机坐标系的值可以由4个控制点表示。

控制点选取参考:https://blog.csdn.net/jessecw79/article/details/82945918

直接求解:ui为空间点pi在图像上的2d投影。K为摄像头的内参矩阵。
位姿估计之EPnP算法学习笔记_第3张图片
带入
在这里插入图片描述
并且把K写成焦距和焦点的形式,有:
在这里插入图片描述
化简就会得到两个方程组:
在这里插入图片描述
在这里插入图片描述
如果把n个点串联起来,可以得到一个线性方程组:
Mx = 0
其中:
在这里插入图片描述
(表示的是控制点在摄像头坐标系下的坐标(X,Y,Z))
那么x就是一个12*1的向量,x属于M的右零空间:
在这里插入图片描述
vi是M的N个0特征值对应的N特征向量,对于第i个控制点有:
在这里插入图片描述
Vk是特征向量Vk的第i个3x1的子向量,βi是一个线性组合
我们可以计算出MTM的特征向量得到Vi,【注意】使用MTM比使用M计算量更少,因为MTM是求解常数复杂度,而M是O(n3)的复杂度,计算MTM的复杂度是O(n)。

还需要求出{βi} i=1,2,…,N。

求解β的策略是:摄像机的外参只是描述的是坐标的变换,不会改变两个坐标之间的距离,无论是在相机坐标系还是世界坐标系中,都有:在这里插入图片描述
即:
在这里插入图片描述
这是一个关于{βi} i=1,2,…,N的二次方程。这个方程的特点是没有关于{βi} i=1,2,…,N的一次项,如果将这些二次项βiβj替换为βij,那么该方程就是{βij},i,j=1,2,…,N的线性方程。对于4个控制点就可以得到6个这么多个方程。
如果不挖掘任何附加条件,N取不同值的时候,新的线性未知数的个数分别为:
N=1, 线性未知数的个数为1;
N=2,线性未知数的个数为3;
N=3,线性未知数的个数为6;
N=4,线性未知数的个数为10;

N=4 的时候,未知数的个数多于方程的个数了。注意到βabβcd=βaβbβcβd=βa′b′βc′d′ ,其中{a′,b′,c′,d′}是{a,b,c,d}的一个排列,我们可以减少未知数的个数。举例,如果我们求出了β11,β12,β13,那么我们可以得到β23=β12β13/β11 。这样,即使对于N=4,我们也可以求解出{βij} i,j=1,⋯,N了。

.

高斯-牛顿最优化:

优化的目标函数:
在这里插入图片描述

得到β之后,我们就可以计算摄像头位姿了:

1.计算控制点在摄像头参考坐标系下的坐标:
在这里插入图片描述
2.计算3D参考点在摄像头参考坐标系下的坐标:
在这里插入图片描述
3.计算的Pw重心和矩阵A:
在这里插入图片描述
位姿估计之EPnP算法学习笔记_第4张图片
4.计算的Pc重心和矩阵B":
在这里插入图片描述
位姿估计之EPnP算法学习笔记_第5张图片
5.计算H
在这里插入图片描述
6.计算H的SVD分解
在这里插入图片描述
7.计算位姿中的旋转R:
在这里插入图片描述
8.计算位姿中的平移t:

在这里插入图片描述
参考:
https://www.jianshu.com/p/b3e9fb2ad0dc
https://so.csdn.net/so/search/s.do?q=%E4%BD%8D%E5%A7%BF&t=blog&u=App_12062011

你可能感兴趣的:(位姿估计之EPnP算法学习笔记)