SLAM ORB-SLAM2(21)基础矩阵的计算和评分
- 1. 前言
- 2. 基础矩阵
- 2.1. 对级约束
- 2.2. 推导
- 2.3. 计算原理
- 3. ComputeF21
- 4. CheckFundamental
1. 前言
在 《SLAM ORB-SLAM2(20)查找基础矩阵》 中了解到 查找基础矩阵主要过程:
- 特征点坐标归一化 Normalize
函数 Normalize 参考 《SLAM ORB-SLAM2(14)特征点坐标归一化》
- 选择归一化之后的特征点
- 八点法计算基础矩阵 ComputeF21
- 评分并评优 CheckFundamental
现在来看看基础矩阵如何计算和评分
2. 基础矩阵
2.1. 对级约束
不过先来了解一下什么是对级约束
拿着相机分别在点 O 1 O_1 O1 和 O 2 O_2 O2 观测空间中一点 P P P
该点在两个视图平面上分别被投影到了点 p 1 p_1 p1 和 p 2 p_2 p2
由于两次测量都是对同一个点 P P P 的观测
所以 O 1 p 1 O_1p_1 O1p1 和 O 2 p 2 O_2p_2 O2p2的延长线相交与点 P P P
即点 P , O 1 , O 2 , p 1 , p 2 P,O_1,O_2,p_1,p_2 P,O1,O2,p1,p2 都在一个平面上,这个平面被称为 极面 (epipolar plane)
连线 O 1 O 2 O_1O_2 O1O2 被称为 基线 (baseline)
基线分别与两个像平面相交与点 e 1 , e 2 e_1,e_2 e1,e2 ,这两个点被称为 极点 (epipole)
极点与成像点 p 1 , p 2 p_1,p_2 p1,p2 的连线 p 1 e 1 , p 2 e 2 p_1e_1,p_2e_2 p1e1,p2e2 所在的直线 l 1 , l 2 l_1,l_2 l1,l2 被称为 极线 (epipolar line)
对级约束就是,在两个不同的位置上观测空间中同一个点,其成像一定在极线上
2.2. 推导
假设某个特征点 P P P
相对于参考帧相机坐标系的坐标为 X 1 = [ x 1 y 1 z 1 ] T \boldsymbol{X_1} = \begin{bmatrix} x_1 & y_1 & z_1 \end{bmatrix}^T X1=[x1y1z1]T
成像点的齐次坐标为 x 1 = [ u 1 v 1 1 ] T \boldsymbol{x_1} = \begin{bmatrix} u_1 & v_1 & 1\end{bmatrix}^T x1=[u1v11]T
根据针孔相机模型:
x 1 = 1 z K X 1 ⟺ [ u 1 v 1 1 ] = 1 z [ f x 0 c x 0 f y c y 0 0 1 ] [ x 1 y 1 z 1 ] \boldsymbol{x_1} = \cfrac{1}{z} \boldsymbol{KX_1} \Longleftrightarrow \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} = \cfrac{1}{z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_1 \\ y_1 \\ z_1 \end{bmatrix} x1=z1KX1⟺⎣⎡u1v11⎦⎤=z1⎣⎡fx000fy0cxcy1⎦⎤⎣⎡x1y1z1⎦⎤ 矩阵 K K K 为相机的内参矩阵,记录了相机的 x y x\ y x y 轴上的焦距 f x , f y f_x, f_y fx,fy 和 光心坐标 c x , c y c_x,c_y cx,cy
此时,考虑相机的内参,将 X 1 , X 2 X_1,X_2 X1,X2投影到成像平面上有:
{ x 1 = 1 z 1 K X 1 x 2 = 1 z 2 K X 2 ⇒ { X 1 = z 1 K − 1 x 1 X 2 = z 2 K − 1 x 2 \begin{cases} \boldsymbol{x_1} = \cfrac{1}{z_1} \boldsymbol{K} \boldsymbol{X_1} \\ \boldsymbol{x_2} = \cfrac{1}{z_2} \boldsymbol{K} \boldsymbol{X_2} \end{cases} \Rightarrow \begin{cases} \boldsymbol{X_1} = z_1\boldsymbol{K}^{-1} \boldsymbol{x_1} \\ \boldsymbol{X_2} = z_2\boldsymbol{K}^{-1} \boldsymbol{x_2} \end{cases} ⎩⎪⎪⎨⎪⎪⎧x1=z11KX1x2=z21KX2⇒{X1=z1K−1x1X2=z2K−1x2
相机经过位姿变换 ⟨ R , t ⟩ ⟨R,t⟩ ⟨R,t⟩后,观测到 P P P 点坐标为,上式导入运动关系:
X 2 = R X 1 + t ⇒ z 2 K − 1 x 2 = z 1 R K − 1 x 1 + t \boldsymbol{X_2} = \boldsymbol{RX_1} + \boldsymbol{t} \ \ \ \ \ \Rightarrow \ \ \ z_2 \boldsymbol{K}^{-1} \boldsymbol{x_2} = z_1 \boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x_1} + \boldsymbol{t}\\ X2=RX1+t ⇒ z2K−1x2=z1RK−1x1+t
两边叉乘 t t t
z 2 t × K − 1 x 2 = z 1 t × R K − 1 x 1 z_2 \boldsymbol{t}_{\times}\boldsymbol{K}^{-1}\boldsymbol{x_2} = z_1 \boldsymbol{t}{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x_1} z2t×K−1x2=z1t×RK−1x1
叉乘(外积): A X B = |A| |B| s i n θ sin\theta sinθ, 向量形成的平行四边形的面积,也是法向量
t = [ a 1 a 2 a 3 ] ⇒ t ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] t=\left[\begin{array}{l}a_{1} \\ a_{2} \\ a_{3}\end{array}\right] \Rightarrow \ t^{\wedge}=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0\end{array}\right] t=⎣⎡a1a2a3⎦⎤⇒ t∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
反对称矩阵,它的主对角线上的元素全为0,而位于主对角线两侧对称的元素反号
A X B = [ 0 − z a y a z a 0 − x a − y a x a 0 ] \begin{bmatrix}0 & -z_a & y_a \\z_a &0 & -x_a \\-y_a & x_a & 0 \end{bmatrix} ⎣⎡0za−ya−za0xaya−xa0⎦⎤ [ x b y b z b ] \begin{bmatrix}x_b \\y_b \\z_b \end{bmatrix} ⎣⎡xbybzb⎦⎤= [ y a z b − y b z a x b z a − x a z b x a y b − x b y a ] \begin{bmatrix}y_az_b-y_bz_a \\x_bz_a-x_az_b \\x_ay_b-x_by_a \end{bmatrix} ⎣⎡yazb−ybzaxbza−xazbxayb−xbya⎦⎤
那么两个平行的向量叉乘为0
两边点乘 ( K − 1 x 2 ) T \left(\boldsymbol{K}^{-1}\boldsymbol{x_2}\right)^T (K−1x2)T
z 2 ( K − 1 x 2 ) T t × K − 1 x 2 = z 1 ( K − 1 x 2 ) T t × R K − 1 x 1 z_2 \left(\boldsymbol{K}^{-1}\boldsymbol{x_2}\right)^T\boldsymbol{t}{\times}\boldsymbol{K}^{-1}\boldsymbol{x_2} = z_1 \left(\boldsymbol{K}^{-1}\boldsymbol{x_2}\right)^T\boldsymbol{t}{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x_1} z2(K−1x2)Tt×K−1x2=z1(K−1x2)Tt×RK−1x1
关注式子的左侧的左边, t × K − 1 x 2 \boldsymbol{t}_{\times}\boldsymbol{K}^{-1}\boldsymbol{x_2} t×K−1x2的含义就是向量 t t t 与 K − 1 x 2 \boldsymbol{K}^{-1}\boldsymbol{x_2} K−1x2 的叉乘
将得到一个同时与这两个向量垂直的向量,该向量与 K − 1 x 2 \boldsymbol{K}^{-1}\boldsymbol{x_2} K−1x2 的点积为零
所以上式可以写成如下的形式:
z 1 ( K − 1 x 2 ) T t × R K − 1 x 1 = 0 z_1 \left(\boldsymbol{K}^{-1}\boldsymbol{x_2}\right)^T\boldsymbol{t}{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x_1}=0 z1(K−1x2)Tt×RK−1x1=0
上式就是一个等式为零的约束,所以其中 z z z的取值实际上没有什么作用,只要非零就行,那么
x 2 T K − T t × R K − 1 x 1 = 0 \boldsymbol{x_2}^T\boldsymbol{K}^{-T}\boldsymbol{t}{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x_1}=0 x2TK−Tt×RK−1x1=0
把矩阵 K − T t × R K − 1 \boldsymbol{K}^{-T}\boldsymbol{t}_{\times}\boldsymbol{R}\boldsymbol{K}^{-1} K−Tt×RK−1 称为 基础矩阵,用符号 F \boldsymbol{F} F 表示,那么得到映射关系:
x 2 T F x = 0 \boldsymbol{x_2}^T \boldsymbol{Fx} = 0 x2TFx=0
2.3. 计算原理
根据两帧图像之间的映射关系: x 2 T F x = 0 \boldsymbol{x_2}^T \boldsymbol{Fx} = 0 x2TFx=0
代入求解:
[ u 2 v 2 1 ] [ f 11 f 12 f 13 f 21 f 22 f 23 f 31 f 32 f 33 ] [ u 1 v 1 1 ] = 0 ⇒ u 2 u 1 f 11 + u 2 v 1 f 12 + u 2 f 13 + v 2 u 1 f 21 + v 2 v 1 f 22 + v 2 f 23 + u 1 f 31 + v 1 f 32 + f 33 = 0 \begin{bmatrix} u_2 & v_2 & 1 \end{bmatrix} \begin{bmatrix} f_{11} & f_{12} & f_{13} \\ f_{21} & f_{22} & f_{23} \\ f_{31} & f_{32} & f_{33} \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} = 0 \ \ \ \ \ \Rightarrow \\ \ \\ u_2u_1f_{11} + u_2v_1f_{12} + u_2f_{13} + v_2u_1f_{21} + v_2v_1f_{22} + v_2f_{23} + u_1f_{31} + v_1f_{32} + f_{33} = 0 [u2v21]⎣⎡f11f21f31f12f22f32f13f23f33⎦⎤⎣⎡u1v11⎦⎤=0 ⇒ u2u1f11+u2v1f12+u2f13+v2u1f21+v2v1f22+v2f23+u1f31+v1f32+f33=0
记 f = [ f 11 f 12 f 13 f 21 f 22 f 23 f 31 f 32 f 33 ] T \boldsymbol{f} = \begin{bmatrix} f_{11} & f_{12} & f_{13} & f_{21} & f_{22} & f_{23} & f_{31} & f_{32} & f_{33} \end{bmatrix}^T f=[f11f12f13f21f22f23f31f32f33]T ,上式可以写成向量点乘的形式:
[ u 2 u 1 u 2 v 1 u 2 v 2 u 1 v 2 v 1 v 2 u 1 v 1 1 ] f = 0 \begin{bmatrix} u_2u_1 & u_2v_1 & u_2 & v_2u_1 & v_2v_1 & v_2 & u_1 & v_1 & 1 \end{bmatrix} \boldsymbol{f} = 0 [u2u1u2v1u2v2u1v2v1v2u1v11]f=0
假有 m m m 对匹配点,根据上式可以写出 m m m 个约束,可以写成 A f = 0 \boldsymbol{Af}=\boldsymbol{0} Af=0 的矩阵形式,如下:
A f = [ u 1 , 2 u 1 , 1 u 1 , 2 v 1 , 1 u 1 , 2 v 1 , 2 u 1 , 1 v 1 , 2 v 1 , 1 v 1 , 2 u 1 , 1 v 1 , 1 1 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ u m , 2 u m , 1 u m , 2 v m , 1 u m , 2 v m , 2 u m , 1 v m , 2 v m , 1 v m , 2 u m , 1 v m , 1 1 ] f = 0 \boldsymbol{Af} = \begin{bmatrix} u_{1,2}u_{1,1} & u_{1,2}v_{1,1} & u_{1,2} & v_{1,2}u_{1,1} & v_{1,2}v_{1,1} & v_{1,2} & u_{1,1} & v_{1,1} & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_{m,2}u_{m,1} & u_{m,2}v_{m,1} & u_{m,2} & v_{m,2}u_{m,1} & v_{m,2}v_{m,1} & v_{m,2} & u_{m,1} & v_{m,1} & 1 \\ \end{bmatrix} \boldsymbol{f} = \boldsymbol{0} Af=⎣⎢⎡u1,2u1,1⋮um,2um,1u1,2v1,1⋮um,2vm,1u1,2⋮um,2v1,2u1,1⋮vm,2um,1v1,2v1,1⋮vm,2vm,1v1,2⋮vm,2u1,1⋮um,1v1,1⋮vm,111⎦⎥⎤f=0
对矩阵 A A A 进行奇异值分解,得到的 右奇异矩阵的最后一列 就是 f \boldsymbol{f} f 的 最优解
能够最小化 ∥ A f ∥ / ∥ f ∥ \boldsymbol{∥Af∥/∥f∥} ∥Af∥/∥f∥的解,使 A f \boldsymbol{Af} Af 尽可能的接近 0
为何右奇异矩阵的最后一列为单应矩阵的最优解 参考 《SLAM ORB-SLAM2(16)奇异值分解》 的 求解超定方程
至少需要8个点才能求得基础矩阵,这也就是所谓的八点法
3. ComputeF21
用 直接线性法 DLT 方法求解 基础矩阵 F F F
按上述公式填充数据于系数矩阵 A A A
/*** @brief 基础矩阵计算求解函数* @param vP1 参考帧归一化坐标* @param vP2 当前帧归一化坐标* @return cv::Mat 计算的基础矩阵F*/
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{/* 获取坐标数量 */const int N = vP1.size();/* 定义系数矩阵A */cv::Mat A(N, 9, CV_32F);/* 配置系数 */for (int i = 0; i < N; i++){/* 获取特征点对的像素坐标 */const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;/* 配置当前特征点对应的约束 */A.at<float>(i, 0) = u2 * u1;A.at<float>(i, 1) = u2 * v1;A.at<float>(i, 2) = u2;A.at<float>(i, 3) = v2 * u1;A.at<float>(i, 4) = v2 * v1;A.at<float>(i, 5) = v2;A.at<float>(i, 6) = u1;A.at<float>(i, 7) = v1;A.at<float>(i, 8) = 1;}/* 定义输出变量,u是左边的正交矩阵U, w为奇异矩阵,vt为右正交矩阵V的转置T */cv::Mat u, w, vt;/* 奇异值分解 */cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);/* 右奇异矩阵的最后一列,重整为基础矩阵 */cv::Mat Fpre = vt.row(8).reshape(0, 3);/* 对初步得来的基础矩阵进行第2次奇异值分解 */cv::SVDecomp(Fpre, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);/* 基础矩阵的秩为2,强制将第3个奇异值设置为0 */w.at<float>(2) = 0;/* 返回重组满足秩约束的基础矩阵 */return u * cv::Mat::diag(w) * vt;
}
通过OpenCV的接口对矩阵 A A A 进行 SVD分解,取最小的奇异值在 V T V^T VT空间中对应的行向量构建基础矩阵
但是基础矩阵的秩只有2,也就是它应当有一个为0的奇异值
对矩阵 A A A 进行 SVD分解 时并不能保证这一点
所以构建矩阵 Fpre
再次进行 SVD分解,令最小的奇异值为 0,再重组基础矩阵
秩:矩阵 A A A 的 非零特征值 的 个数
奇异值分解 参考 《SLAM ORB-SLAM2(16)奇异值分解》
4. CheckFundamental
经过计算,得到基础矩阵,那么根据基础矩阵计算重投影误差
这里返回的单应矩阵是归一化之后的,还不能直接使用, 需要恢复归一化之前的矩阵才行
在 《SLAM ORB-SLAM2(20)查找基础矩阵》 的 5. 八点法计算基础矩阵 中已经恢复了:
cv::Mat Fn = ComputeF21(vPn1i, vPn2i); /* 八点法计算基础矩阵 */
F21i = T2t * Fn * T1; /* 根据归一化的矩阵恢复 基础矩阵 F21 */
在第一步已经进行特征点归一化:vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2
又因为 x x x2T F F F21 x x x1=0
根据基础矩阵约束得到:(T2 * mvKeys2)T * Fn * T1 * mvKeys1 = 0
进一步得到 : mvKeys2T * T2t * Fn * T1 * mvKeys1 = 0
/*** @brief 基础矩阵评分函数* @param F21 参考帧到当前帧的基础矩阵* @param vbMatchesInliers 特征点对的Inlier标记* @param sigma 标准差* @return score 得分*/
float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
{/* 获取匹配点对数量 */const int N = mvMatches12.size();/* 获取从参考帧到当前帧的基础矩阵的各个元素 */const float f11 = F21.at<float>(0, 0);const float f12 = F21.at<float>(0, 1);const float f13 = F21.at<float>(0, 2);const float f21 = F21.at<float>(1, 0);const float f22 = F21.at<float>(1, 1);const float f23 = F21.at<float>(1, 2);const float f31 = F21.at<float>(2, 0);const float f32 = F21.at<float>(2, 1);const float f33 = F21.at<float>(2, 2);
获取从参考帧到当前帧的基础矩阵中的各个元素
/* 预分配特征点对Inliers标记的空间 */vbMatchesInliers.resize(N);/* 初始化单应矩阵得分 */float score = 0;/* 基于卡方检验计算出的阈值(假设测量有一个像素的偏差)自由度为1的卡方分布在显著性水平为0.05时对应的临界阈值为3.841自由度为2的卡方分布在显著性水平为0.05时对应的临界阈值为5.991 */const float th = 3.841;const float thScore = 5.991;/* 方差平方的倒数,作为信息矩阵 协方差的逆 */const float invSigmaSquare = 1.0 / (sigma * sigma);
根据传入的sigma值计算协方差矩阵
/* 通过F矩阵,进行参考帧和当前帧之间的双向投影,并计算出加权重投影误差 */for (int i = 0; i < N; i++){/* 开始都默认为Inlier */bool bIn = true;/* 提取参考帧和当前帧之间的特征匹配点对 */const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];/* 提取出特征点的坐标 */const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Reprojection error in second image// l2=F21x1=(a2,b2,c2)/* 计算 参考帧 上的点在 当前帧 上投影得到的极线 l2 = F21 * p1 = (a2, b2, c2) */const float a2 = f11 * u1 + f12 * v1 + f13;const float b2 = f21 * u1 + f22 * v1 + f23;const float c2 = f31 * u1 + f32 * v1 + f33;/* 计算 当前帧的特征点 到 投影得到的极线 l2 的距离平方 d^2 = ((ax+by+c) / sqrt(a^2 + b^2))^2 */const float num2 = a2 * u2 + b2 * v2 + c2;const float squareDist1 = num2 * num2 / (a2 * a2 + b2 * b2);/* 转换参考帧投影至当前帧的误差 */const float chiSquare1 = squareDist1 * invSigmaSquare;/* 阀值用th,而累加得分用thScore,确保与单应基础评分标准一致 *//* 用阈值标记离群点,内点的话累加得分 */if (chiSquare1 > th)bIn = false;else/* 累计得分。误差越大,得分越低 */score += thScore - chiSquare1;// Reprojection error in second image// l1 =x2tF21=(a1,b1,c1)/* 计算 当前帧 上的点在 参考帧 上投影得到的极线 l1 = p2 * F21 = (a1, b1, c1) */const float a1 = f11 * u2 + f21 * v2 + f31;const float b1 = f12 * u2 + f22 * v2 + f32;const float c1 = f13 * u2 + f23 * v2 + f33;/* 计算 参考帧的特征点 到 投影得到的极线 l1 的距离平方 d^2 = ((ax+by+c) / sqrt(a^2 + b^2))^2 */const float num1 = a1 * u1 + b1 * v1 + c1;const float squareDist2 = num1 * num1 / (a1 * a1 + b1 * b1);/* 转换当前帧投影至参考帧的误差 */const float chiSquare2 = squareDist2 * invSigmaSquare;/* 用阈值标记离群点,内点的话累加得分 */if (chiSquare2 > th)bIn = false;else/* 累计得分。误差越大,得分越低 */score += thScore - chiSquare2;/* 双向重投影误差均满足要求,则说明当前点为内点 */if (bIn)vbMatchesInliers[i] = true;elsevbMatchesInliers[i] = false;}/* 返回得分 */return score;
}
根据对极约束的几何意义,它能够将一幅图像中的某个点投影到另一幅图像的极线上
对每个匹配的特征点,计算双向投影误差(特征点 到 投影得到的极线 的距离 d = d = d= a x + b y + c a 2 + b 2 ax+by+c \over \sqrt{a^2 + b^2} a2+b2ax+by+c 的 平方),并获取对应得分和更新内点标记,具体如下:
- 获取匹配点的像素坐标
- 计算参考帧上的点 x 1 x_1 x1 在当前帧上投影得到的极线 l 2 l_2 l2,再计算参考帧到当前帧的带权重的重投影误差,同时累积当前得分并进行内点判断
- 计算当前帧上的点 x 2 x_2 x2 在参考帧上投影得到的极线 l 1 l_1 l1,再计算当前帧到参考帧的带权重的重投影误差,同时累积当前得分并进行内点判断
- 记录当前匹配点的内点标记