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;
else
vbMatchesInliers[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，再计算当前帧到参考帧的带权重的重投影误差，同时累积当前得分并进行内点判断
- 记录当前匹配点的内点标记
