SLAM系列--RANSAC和EPNP算法
作者:Frank
时间:2016-09-03
在Feature-Based SLAM中, 在利用特征检测算子对前后两帧进行了特征检测和匹配后,得到的结果可能有很大的误差,因为可能会存在误匹配的情况,这会使得计算获取的位姿精度降低,因此我们需要对匹配结果进行滤波和优化,而在此过程中最经典的就是利用RANSAC(RANdom SAmple Consensus ,随机采样一致性)算法。而在优化了匹配结果后,我们需要利用该结果来计算两帧之间的相对运动(即旋转R和平移t),而这部分可以利用本节将要讲述的EPNP算法来进行求解。因此,本节会按照上述的顺序先介绍RANSAC算法的基本原理,然后再介绍EPNP算法的原理。
1. RANSAC算法
1.1. RANSAC算法基础
1.1.1. 算法的基本假设
- 数据是由”局内点”组成,例如:数据的分布可以用一些模型参数来解释;
- “局外点”是不能适应该模型的数据;
- 除此之外的数据属于噪声。
局外点产生的原因有:噪声的极值;错误的测量方法;对数据的错误假设。
RANSAC也做了以下假设:给定一组(通常很小的)局内点,存在一个可以估计模型参数的过程,而该模型能够解释或者适用于局内点。
1.1.2. 算法示例
一个简单的例子就是从一组观测数据中找出合适的二维直线。假设观测数据中包含局内点和局外点,其中局内点近似的被直线所通过,而局外点远离直线。简单的最小二乘法不能找到适应于局内点的直线,原因是最小二乘法尽量去适应包括局外点在内的所有点。相反,RANSAC能得出一个仅仅利用局内点计算出模型,并且概率还足够高。但是,RANSAC并不能保证结果一定正确,为了保证算法有足够高的合理概率,我们必须小心的选择算法的参数。图示如下所示:
上一张图表示包含很多局外点的数据集,而右图则是RANSAC找到的直线(局外点并不会影响结果)。
1.1.3. 算法概述
RANSAC算法的输入是一组观测数据,一个可以解释或者适应于观测数据的参数化模型,一些可信的参数。
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点,并用下述方法进行验证:
- 有一个模型适应于假设的局内点,即所有的未知参数都能从假设的局内点计算得出;
- 用1中得到的模型去测试所有的其他数据,如果某个点适应于估计的模型,认为它也是局内点。
- 如果有足够多的点被归类于假设的局内点,那么估计的模型就足够合理;
- 然后,用所有假设的局内点去重新估计模型,因为它仅仅被初始的假设局内点估计过;
- 最后,通过估计局内点与模型的错误率来评估模型。
这个过程被重复执行固定的次数,每次产生的模型要么因为局内点太少而被舍弃,要么因为比现在的模型更好而被选用。
1.1.4. RANSAC的优劣
RANSAC的优点:是能鲁棒的估计模型参数。例如,它能从包含大量局外点的数据集中估计出高精度的参数。
RANSAC的缺点:计算参数的迭代次数没有上限,如果设置迭代次数的上限,得到的结果可能不是最优的结果,甚至可能得到错误的结果。RANSAC只有一定的概率得到可信的模型,概率与迭代次数成正比。另一个缺点是它要求设置跟问题相关的阈值。而且RANSAC只能从特定的数据集中估计出一个模型,如果存在两个(或多个)模型,RANSAC不能找到别的模型。
总之,在一组包含局内点的数据集中,采用不断迭代的方法寻找参数模型。
1.2. RANSAC算法用于消除图像误匹配原理
1.2.1. 算法原理
利用RANSAC算法来消除图像误匹配的原理,是利用RANSAC算法来寻找一个最佳单应性矩阵,矩阵大小为$3 \times 3$,目的是找到一个最优的参数矩阵,使得满足该矩阵的数据点数最多,通常令$ h_{33}=1$,由于单应性矩阵有8个位置参数,所以需要8个线性方程求解。对应到点位置信息上,一组点对可以列出两个方程,则至少包含4组匹配点对。,公式如下所示:(也可以 关于张正友标定 的讲述中找到该部分)
$$ s \left[\matrix{ x’ \cr y’ \cr 1 \cr}\right]=\left[\matrix{h_{11}&h_{12}&h_{13} \cr h_{21}&h_{22}&h_{23} \cr h_{31}&h_{32}&1 \cr}\right] \left[\matrix{x \cr y \cr 1 \cr}\right] \tag{1} $$
其中,$(x,y) $表示目标图像的角点位置,$(x’,y’)$为场景图像角点位置。s为尺度参数。
RANSAC算法从匹配数据集中随机抽取四个样本并保证这四个样本之间不共线。计算出单应性矩阵,然后利用这个模型测试所有数据,并计算满足这个模型数据点的个数和投影误差(即代价函数)若此模型为最优模型,则对应的代价函数最小:
$$\sum_{i=1}^{n}((x_i^{‘}{h_{11}x_i+h_{12}y_i+h_{13} \over h_{31}x_i+h_{32}y_i+h_{33} })^2+(y_i^{‘}{h_{21}x_i+h_{22}y_i+h_{23} \over h_{31}x_i+h_{32}y_i+h_{33} })^2) \tag{2} $$
1.2.2. 算法步骤
- 随机从数据集中随机抽出4个样本数据(此四个样本之间不共线)计算出变换矩阵H,记为模型M;
- 计算数据集中所有数据与模型M的投影误差,若误差小于阈值,加入内点集I;
- 如果当前内点集元素个数大于最优内点集$I_{best}$,则更新$I_{best}=I$,同事更新迭代次数k;
- 如果迭代次数大于k,则退出;否则迭代次数加1,并重复上述步骤;
注:迭代次数k在不大于最大迭代次数的情况下,是在不断更新而不是固定的。
$$ k={log(1-p) \over log(1-w^m)} \tag{3} $$
其中,p为置信度,一般取为0.995,w为内点的比例,m为计算模型所需要的最小样本数;
** 转载来源:随机抽样一致性算法(RANSAC);
2. EPnP算法
PnP是利用已知匹配点对以及相机内参来求解相机位姿的算法,而EPnP则是针对$n \geq 3$情况下相机位姿求解的O(n)时间的算法。
2.1. 基本表示
相机坐标用$F^c$表示,世界坐标系用$F^w$表示,任何一点可以用四个控制点$c_j$表示,其中,世界坐标系中的点$p_i^w$可以表示为:
$$p_i^w=\sum_{j=1}^{4} \alpha _{ij} c_j^w , with \sum_{j=1}^{4}\alpha _{ij}=1 \tag{4} $$
对于相机坐标系中的点$ p_i^c$,有:
$$ p_i^c=\sum_{j=1}^{4} \alpha _{ij} c_j^c , with \sum_{j=1}^{4}\alpha _{ij}=1 \tag{5} $$
对于上面的公式来说,首先需要说明的是$ \alpha _{ij}$确实存在,因为$c_j^w$构成的方程组是非正定的,所以一定存在解。
理论上来说,控制点可以随便选择,这里选择控制点为参考点的中心,其他的点在PCA得到的主轴上单位长度处,从而提高算法的稳定性。
2.2. 控制点在相机坐标系的坐标
根据投影方程得到世界坐标系中参考点坐标和相机坐标系中参考点的约束关系:
$$ \forall i, w_i\left[\matrix{u_i \cr v_i \cr 1 \cr}\right]=A p_i^c=A\sum_{j=1}^{4}\alpha _{ij}c_j^c \tag{6} $$
写成矩阵的形式为:
$$ \forall i, w_i\left[\matrix{u_i \cr v_i \cr 1 \cr}\right]=\left[\matrix{f_u & 0 & u_c \cr 0 & f_v & v_c \cr 0 & 0 & 1 \cr}\right] \sum_{j=1}^{4} \alpha _{ij} \left[\matrix{x_j^c \cr y_j^c \cr z_j^c \cr}\right] \tag{7} $$
将等式拆解,从第三行得到:
$$ w_i=\sum _{j=1}^{4} \alpha_{ij} z_j^c \tag{8} $$
将$w_i$代入一二行,可以得到如下等式:
$$ \matrix{
\sum_{j=1}^{4} \alpha _{ij} f_u x_j^c + \alpha _{ij} (u_c-u_i)z_j^c =0 \cr
\sum_{j=1}^{4} \alpha _{ij} f_v y_j^c + \alpha _{ij} (v_c-v_i)z_j^c =0 \cr
} \tag{9} $$
因此,可以得到如下线性方程组:
$$ x={\left[\matrix{c_1^{cT} & c_2^{cT} & c_3^{cT} & c_4^{cT} \cr}\right]}^T \tag{10} $$
上面的方程中,四个控制点总共12个未知变量,M为$2n \times 12 $的矩阵。因此,x属于M的右零空间,$v_i$为矩阵M的右奇异向量,可以通过求解$M^TM$的零空间的特征值得到。
$$ x=\sum_{i=1}^{N} \beta _i v_i \tag{11} $$
说明:使用$M^TM$比使用M计算量更小,因为$M^TM$的求解释常数复杂度,而M是$O(n^3)$的复杂度,但是计算$M^TM$的复杂度是$O(n)$的。
2.3. 选择合适的线性组合
上面的求解的x中,需要确定$\beta _i$,也就是确定合适的线性组合。根据参考点的位置不同,矩阵$M^TM$的零空间维度可能为N=1->4维。求解$\beta$的策略是控制点在坐标系$F^w$和$F^c$中,两两之间的距离是相同的,而x的3K+1->3K分量表示不用控制点在相机坐标系中的坐标,总共有$C_4^2=6$个约束。
如果N=1,则根据约束有:
$$||\beta v^{|i|}-\beta v^{|j|}||^2=||c_i^w-c_j^w||^2 \tag{12} $$
所以:
$$\beta ={\sum _{|i,j| \in |i;4|} ||v^{|i|}-v^{|j|}|| \cdot ||c_i^w-c_j^w||} \over \sum _{|i,j| \in |i;4|} ||v^{|i|}-v^{|j|}||^2 \tag{13} $$
如果N=2,
$$||\beta _1 v_1^{[i]}+\beta _2 v_2^{[i]}-(\beta _1 v_1^{[j]}+\beta _2 v_2^{[j]})||^2=||c_i^w-c_j^w||^2 \tag{14}$$
由于$\beta_1$和$\beta_2$只以二次项出现在方程中,记$\beta =\left[\matrix{\beta_1^2 & \beta_1 \beta_2 & \beta_2^2 \cr}\right]^T$,$\rho$的每一项为$||c_i^w-c_j^w||^2$,得到相面的方程:
$$ L\beta =\rho \tag{15} $$
其中L是由$v_1$和$v_2$构成的$6 \times 3$的矩阵。
上面的方程可以通过$\beta=(L^TL)-1L^T \rho$得到,然后通过选择合适的符号从$\beta_1^2 , \beta_1 \beta_2 , \beta_2^2 $使得所有的$p_i^c$有正的z坐标。
如果N=3则和N=2差不多,唯一的区别在于使用的是L的逆,而不是伪逆,此时的L是$6 \times 6$的矩阵。
前面的步骤可以得到目标点在相机坐标系中的闭式解,作为G-N优化的初始值,优化的变量为$\beta=\left[\matrix{\beta_1,\beta_2,\cdots , \beta_N \cr}\right]^T$,目标函数为:
$$ Error(\beta) = \sum_{(i,j)s.t.i<j}(||c_i^c-c_j^c||^2-||c_i^w-c_j^w||^2) \tag{16} $$
该优化过程和参考点的数目无关,优化步骤和时间是常数。
2.4. 计算R,t
前面的两步计算不同维数的零空间的误差,选择误差最小维数对应的$\beta$,从而得到x,恢复出控制点在相机坐标系中的坐标并根据质心坐标系数得到参考点在相机坐标系中的坐标。剩下的工作就是已知一组点云在两个坐标系中的坐标,求两个坐标系的位姿变换。步骤如下:
- 求中心点,$p_c^c={\sum p_c^i \over N},p_w^c={\sum p_w^i \over N}$;
- 去中心,$q_c^i=p_c^i-p_c^c,q_w^i=p_w^i-p_w^c$;
- 计算H矩阵,$H=\sum_{i=1}^{N}q_c^i q_w^{iT} $;
- 对H进行SVD分解,$H=U \Lambda V^T $;
- 计算$X=VU^T$,如果$det(x)=1$,则$R=X$,$t=P_c^c -R P_w^c$。否则$R(2,\cdot)=-R(2,\cdot)$;
转载请注明出处