讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/123092196
文末正下方中心提供了本人
联系方式,
点击本人照片即可显示
W
X
→
官方认证
{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证}
文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
一、前言
在上一篇博客中,我们讲解了 Homography 矩阵的求解过程,该篇博客主要对Fundamental 矩阵进行分析,再讲解之前,请大家详细阅读该篇博客 史上最简SLAM零基础解读(2) - 对极约束→Essential矩阵、Fundamental矩阵推导
通过该篇博客,我们可以了解到如果世界坐标
P
P
P 再两个相机成像平面
I
0
I_0
I0,
I
1
I_1
I1,图像坐标分别为
p
0
=
(
x
0
y
0
1
)
p
1
=
(
x
1
y
1
1
)
\color{blue} p_0=\begin{pmatrix} x_0\\ y_0\\ 1\\ \end{pmatrix} ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~p_1=\begin{pmatrix} x_1\\ y_1\\ 1\\ \end{pmatrix}
p0=⎝
⎛x0y01⎠
⎞ p1=⎝
⎛x1y11⎠
⎞那么必然存在
p
0
⋅
E
p
1
=
p
0
T
E
p
1
=
0
\color{blue}p_0 \cdot Ep_1=p_0^T Ep_1=0
p0⋅Ep1=p0TEp1=0 其中
E
=
t
×
R
=
t
∧
R
\color{blue} E=t\times R=t ^{\wedge} R
E=t×R=t∧R上式
t
t
t 是光心
O
1
O_1
O1 相对于
O
0
O_0
O0 的平移 ,
E
E
E 我们称为本质或者本征矩阵(Essential),
E
p
1
Ep1
Ep1 为极线方程。 总的来说就是平面
I
0
I_0
I0 中的任意一点,一定在平面
I
1
I_1
I1 的极线
E
p
1
Ep_1
Ep1上。另外我们还进一步推导出来基本矩阵:
F
=
(
K
0
−
T
E
K
1
−
1
)
u
o
T
F
v
1
=
0
\color{blue} F=(K_0^{- T} EK_1^{-1}) ~~~~~~~~~~ \color{blue} u_o^TFv_1=0
F=(K0−TEK1−1) uoTFv1=0其上的 注意,其上的
p
0
,
p
1
p_0,p_1
p0,p1 是图像坐标,
v
0
,
v
1
v_0,v_1
v0,v1 是像素坐标。我们源码中的特征点肯定是像素坐标,所以我们需要求解基本矩阵。但是上述公式中其为一对特征点建立的 欠定方程。我们需要需要把对点对齐进行约束,这里假设特征点对(像素坐标)如下:
p
i
=
(
x
i
y
i
1
)
p
i
′
=
(
x
i
′
y
i
′
1
)
\color{blue} p_{i}=\begin{pmatrix} x_{i}\\ y_{i}\\ 1\\ \end{pmatrix} ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~p'_{i}=\begin{pmatrix} x'_{i}\\ y'_{i}\\ 1\\ \end{pmatrix}
pi=⎝
⎛xiyi1⎠
⎞ pi′=⎝
⎛xi′yi′1⎠
⎞那么带入
v
o
T
F
v
1
=
0
v_o^TFv_1=0
voTFv1=0 ,令
v
0
=
p
i
′
,
v
1
=
p
i
v_0=p'_i, v_1=p_i
v0=pi′,v1=pi,然后展开如下:
[
x
i
′
y
i
′
1
]
[
f
1
f
2
f
3
f
4
f
5
f
6
f
7
f
8
f
9
]
[
x
i
y
i
1
]
=
0
\color{blue} \left[\begin{array}{lll} x'_{i} & y'_{i} & 1 \end{array}\right]\left[\begin{array}{lll} f_{1} & f_{2} & f_{3} \\ f_{4} & f_{5} & f_{6} \\ f_{7} & f_{8} & f_{9} \end{array}\right]\left[\begin{array}{c} x_{i} \\ y_{i} \\ 1 \end{array}\right]=0
[xi′yi′1]⎣
⎡f1f4f7f2f5f8f3f6f9⎦
⎤⎣
⎡xiyi1⎦
⎤=0为了方便得
a
=
f
1
∗
x
′
+
f
4
∗
y
′
+
f
7
b
=
f
2
∗
x
′
+
f
5
∗
y
′
+
f
8
c
=
f
3
∗
x
′
+
f
6
∗
y
′
+
f
9
\color{blue} \begin{array}{l} a=f_{1} * x'+f_{4} * y'+f_{7} \\ b=f_{2} * x'+f_{5} * y'+f_{8} \\ c=f_{3} * x'+f_{6} * y'+f_{9} \\ \end{array}
a=f1∗x′+f4∗y′+f7b=f2∗x′+f5∗y′+f8c=f3∗x′+f6∗y′+f9那么上面的矩阵可以转换为:
[
a
b
c
]
[
x
i
y
i
1
]
=
0
\color{blue} \left[\begin{array}{lll} a & b & c \end{array}\right]\left[\begin{array}{c} x_{i} \\ y_{i} \\ 1 \end{array}\right]=0
[abc]⎣
⎡xiyi1⎦
⎤=0
f
1
x
i
x
i
′
+
f
2
y
i
x
i
′
+
f
3
x
i
′
+
f
4
x
i
y
i
′
+
f
5
y
i
y
i
′
+
f
6
y
i
′
+
f
7
x
i
+
f
8
y
i
+
f
9
=
0
\color{blue} f_{1}x_{i}x_{i}'+f_{2}y_{i}x_{i}'+f_{3}x_{i}'+f_{4}x_{i}y_{i}'+f_{5}y_{i}y_{i}'+f_{6}y_{i}'+f_{7}x_{i}+f_{8}y_{i}+f_{9}=0
f1xixi′+f2yixi′+f3xi′+f4xiyi′+f5yiyi′+f6yi′+f7xi+f8yi+f9=0 转化为矩阵形式:
[
x
i
x
i
′
y
i
x
i
′
x
i
′
x
i
y
i
′
y
i
y
i
′
y
i
′
x
i
y
i
1
]
[
f
1
f
1
f
3
f
4
f
5
f
6
f
7
f
8
f
9
]
=
0
\color{blue} \left[\begin{array}{ccccccccc} x_{i}x_{i}' & y_{i}x_{i}' & x_{i}'& x_{i}y_{i}' & y_{i}y_{i}' & y_{i}' & x_{i} & y_{i} & 1\\ \end{array}\right]\left[\begin{array}{l} f_{1} \\ f_{1} \\ f_{3} \\ f_{4} \\ f_{5} \\ f_{6} \\ f_{7} \\ f_{8} \\ f_{9} \end{array}\right]=0
[xixi′yixi′xi′xiyi′yiyi′yi′xiyi1]⎣
⎡f1f1f3f4f5f6f7f8f9⎦
⎤=0
其上为任意一点对组成的方程,如果写成八个点对的公式图下:
[
x
1
x
1
′
y
1
x
1
′
x
1
′
x
1
y
1
′
y
1
y
1
′
y
1
′
x
1
y
1
1
x
2
x
2
′
y
2
x
2
′
x
2
′
x
2
y
2
′
y
2
y
2
′
y
2
′
x
2
y
2
1
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
x
7
x
7
′
y
7
x
7
′
x
7
′
x
7
y
7
′
y
7
y
7
′
y
7
′
x
7
y
7
1
x
8
x
8
′
y
8
x
8
′
x
8
′
x
8
y
8
′
y
8
y
8
′
y
8
′
x
8
y
8
1
]
[
f
1
f
1
f
3
f
4
f
5
f
6
f
7
f
8
f
9
]
=
0
\color{blue} \left[\begin{array}{ccccccccc} x_{1}x_{1}' & y_{1}x_{1}' & x_{1}'& x_{1}y_{1}' & y_{1}y_{1}' & y_{1}' & x_{1} & y_{1} & 1\\ x_{2}x_{2}' & y_{2}x_{2}' & x_{2}'& x_{2}y_{2}' & y_{2}y_{2}' & y_{2}' & x_{2} & y_{2} & 1\\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ x_{7}x_{7}' & y_{7}x_{7}' & x_{7}'& x_{7}y_{7}' & y_{7}y_{7}' & y_{7}' & x_{7} & y_{7} & 1\\ x_{8}x_{8}' & y_{8}x_{8}' & x_{8}'& x_{8}y_{8}' & y_{8}y_{8}' & y_{8}' & x_{8} & y_{8} & 1\\ \end{array}\right]\left[\begin{array}{l} f_{1} \\ f_{1} \\ f_{3} \\ f_{4} \\ f_{5} \\ f_{6} \\ f_{7} \\ f_{8} \\ f_{9} \end{array}\right]=0
⎣
⎡x1x1′x2x2′⋯⋯x7x7′x8x8′y1x1′y2x2′⋯⋯y7x7′y8x8′x1′x2′⋯⋯x7′x8′x1y1′x2y2′⋯⋯x7y7′x8y8′y1y1′y2y2′⋯⋯y7y7′y8y8′y1′y2′⋯⋯y7′y8′x1x2⋯⋯x7x8y1y2⋯⋯y7y811⋯⋯11⎦
⎤⎣
⎡f1f1f3f4f5f6f7f8f9⎦
⎤=0
二、代码流程
代码的主要逻辑如下:
* Step 1 将当前帧和参考帧中的特征点坐标进行归一化
* Step 2 选择8个归一化之后的点对进行迭代
* Step 3 八点法计算基础矩阵矩阵
* Step 4 利用重投影误差为当次RANSAC的结果评分
* Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记
其与上一篇博客求解 Homography 矩阵的流程基本一致,所以这里就不再进行重复了。
三、源码注释
主调函数为 FindFundamental函数,其内部核心函数为:
Normalize() 归一化操作
ComputeF21() 八点法计算计算F矩阵
CheckFundamental() 重投影误差评分
C o m p u t e F 21 ( ) 与 C h e c k F u n d a m e n t a l ( ) 在后面的博客有详细的介绍 \color{red}ComputeF21() 与 CheckFundamental() 在后面的博客有详细的介绍 ComputeF21()与CheckFundamental()在后面的博客有详细的介绍 ,所以大家这里随便看一下,不用太在意 。
void Initializer::FindFundamental() 函数具体实现如下:
/**
* @brief 归一化特征点到同一尺度,作为后续normalize DLT的输入
* [x' y' 1]' = T * [x y 1]'
* 归一化后x', y'的均值为0,sum(abs(x_i'-0))=1,sum(abs((y_i'-0))=1
*
* 为什么要归一化?
* 在相似变换之后(点在不同的坐标系下),他们的单应性矩阵是不相同的
* 如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化
* 我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一
* 对同一幅图像的坐标进行相同的变换,不同图像进行不同变换
* 缩放尺度是为了让噪声对于图像的影响在一个数量级上
*
* Step 1 计算特征点X,Y坐标的均值
* Step 2 计算特征点X,Y坐标离均值的平均偏离程度
* Step 3 将x坐标和y坐标分别进行尺度归一化,使得x坐标和y坐标的一阶绝对矩分别为1
* Step 4 计算归一化矩阵:其实就是前面做的操作用矩阵变换来表示而已
*
* @param[in] vKeys 待归一化的特征点
* @param[in & out] vNormalizedPoints 特征点归一化后的坐标
* @param[in & out] T 归一化特征点的变换矩阵
*/
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T) //将特征点归一化的矩阵
{
// 归一化的是这些点在x方向和在y方向上的一阶绝对矩(随机变量的期望)。
// Step 1 计算特征点X,Y坐标的均值 meanX, meanY
float meanX = 0;
float meanY = 0;
//获取特征点的数量
const int N = vKeys.size();
//设置用来存储归一后特征点的向量大小,和归一化前保持一致
vNormalizedPoints.resize(N);
//开始遍历所有的特征点
for(int i=0; i<N; i++)
{
//分别累加特征点的X、Y坐标
meanX += vKeys[i].pt.x;
meanY += vKeys[i].pt.y;
}
//计算X、Y坐标的均值
meanX = meanX/N;
meanY = meanY/N;
// Step 2 计算特征点X,Y坐标离均值的平均偏离程度 meanDevX, meanDevY,注意不是标准差
float meanDevX = 0;
float meanDevY = 0;
// 将原始特征点减去均值坐标,使x坐标和y坐标均值分别为0
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
//累计这些特征点偏离横纵坐标均值的程度
meanDevX += fabs(vNormalizedPoints[i].x);
meanDevY += fabs(vNormalizedPoints[i].y);
}
// 求出平均到每个点上,其坐标偏离横纵坐标均值的程度;将其倒数作为一个尺度缩放因子
meanDevX = meanDevX/N;
meanDevY = meanDevY/N;
float sX = 1.0/meanDevX;
float sY = 1.0/meanDevY;
// Step 3 将x坐标和y坐标分别进行尺度归一化,使得x坐标和y坐标的一阶绝对矩分别为1
// 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值(期望)
for(int i=0; i<N; i++)
{
//对,就是简单地对特征点的坐标进行进一步的缩放
vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
}
// Step 4 计算归一化矩阵:其实就是前面做的操作用矩阵变换来表示而已
// |sX 0 -meanx*sX|
// |0 sY -meany*sY|
// |0 0 1 |
T = cv::Mat::eye(3,3,CV_32F);
T.at<float>(0,0) = sX;
T.at<float>(1,1) = sY;
T.at<float>(0,2) = -meanX*sX;
T.at<float>(1,2) = -meanY*sY;
}
/**
* @brief 根据特征点匹配求fundamental matrix(normalized 8点法)
* 注意F矩阵有秩为2的约束,所以需要两次SVD分解
*
* @param[in] vP1 参考帧中归一化后的特征点
* @param[in] vP2 当前帧中归一化后的特征点
* @return cv::Mat 最后计算得到的基础矩阵F
*/
cv::Mat Initializer::ComputeF21(
const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{
// 原理详见附件推导
// x'Fx = 0 整理可得:Af = 0
// A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |
// 通过SVD求解Af = 0,A'A最小特征值对应的特征向量即为解
//获取参与计算的特征点对数
const int N = vP1.size();
//初始化A矩阵
cv::Mat A(N,9,CV_32F); // N*9维
// 构造矩阵A,将每个特征点添加到矩阵A中的元素
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;
}
//存储奇异值分解结果的变量
cv::Mat u,w,vt;
// 定义输出变量,u是左边的正交矩阵U, w为奇异矩阵,vt中的t表示是右正交矩阵V的转置
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
// 转换成基础矩阵的形式
cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
//基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2
// 对初步得来的基础矩阵进行第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;
}
/**
* @brief 对给定的Fundamental matrix打分
*
* @param[in] F21 当前帧和参考帧之间的基础矩阵
* @param[in] vbMatchesInliers 匹配的特征点对属于inliers的标记
* @param[in] sigma 方差,默认为1
* @return float 返回得分
*/
float Initializer::CheckFundamental(
const cv::Mat &F21, //当前帧和参考帧之间的基础矩阵
vector<bool> &vbMatchesInliers, //匹配的特征点对属于inliers的标记
float sigma) //方差
{
// 说明:在已值n维观测数据误差服从N(0,sigma)的高斯分布时
// 其误差加权最小二乘结果为 sum_error = SUM(e(i)^T * Q^(-1) * e(i))
// 其中:e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵,即sigma * sigma组成的协方差矩阵
// 误差加权最小二次结果越小,说明观测数据精度越高
// 那么,score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高
// 算法目标:检查基础矩阵
// 检查方式:利用对极几何原理 p2^T * F * p1 = 0
// 假设:三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p1 和 p2(两个为同名点)
// 则:p2 一定存在于极线 l2 上,即 p2*l2 = 0. 而l2 = F*p1 = (a, b, c)^T
// 所以,这里的误差项 e 为 p2 到 极线 l2 的距离,如果在直线上,则 e = 0
// 根据点到直线的距离公式:d = (ax + by + c) / sqrt(a * a + b * b)
// 所以,e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b)
// 算法流程
// input: 基础矩阵 F 左右视图匹配点集 mvKeys1
// do:
// for p1(i), p2(i) in mvKeys:
// l2 = F * p1(i)
// l1 = p2(i) * F
// error_i1 = dist_point_to_line(x2,l2)
// error_i2 = dist_point_to_line(x1,l1)
//
// w1 = 1 / sigma / sigma
// w2 = 1 / sigma / sigma
//
// if error1 < th
// score += thScore - error_i1 * w1
// if error2 < th
// score += thScore - error_i2 * w2
//
// if error_1i > th or error_2i > th
// p1(i), p2(i) are inner points
// vbMatchesInliers(i) = true
// else
// p1(i), p2(i) are outliers
// vbMatchesInliers(i) = false
// end
// end
// output: score, inliers
// 获取匹配的特征点对的总对数
const int N = mvMatches12.size();
// Step 1 提取基础矩阵中的元素数据
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);
// 预分配空间
vbMatchesInliers.resize(N);
// 设置评分初始值(因为后面需要进行这个数值的累计)
float score = 0;
// 基于卡方检验计算出的阈值
// 自由度为1的卡方分布,显著性水平为0.05,对应的临界阈值
// ?是因为点到直线距离是一个自由度吗?
const float th = 3.841;
// 自由度为2的卡方分布,显著性水平为0.05,对应的临界阈值
const float thScore = 5.991;
// 信息矩阵,或 协方差矩阵的逆矩阵
const float invSigmaSquare = 1.0/(sigma*sigma);
// Step 2 计算img1 和 img2 在估计 F 时的score值
for(int i=0; i<N; i++)
{
//默认为这对特征点是Inliers
bool bIn = true;
// Step 2.1 提取参考帧和当前帧之间的特征匹配点对
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
// Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 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;
// Step 2.3 计算误差 e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b)
const float num2 = a2*u2+b2*v2+c2;
const float squareDist1 = num2*num2/(a2*a2+b2*b2);
// 带权重误差
const float chiSquare1 = squareDist1*invSigmaSquare;
// Step 2.4 误差大于阈值就说明这个点是Outlier
// ? 为什么判断阈值用的 th(1自由度),计算得分用的thScore(2自由度)
// ? 可能是为了和CheckHomography 得分统一?
if(chiSquare1>th)
bIn = false;
else
// 误差越大,得分越低
score += thScore - chiSquare1;
// 计算img2上的点在 img1 上投影得到的极线 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;
// 计算误差 e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b)
const float num1 = a1*u1+b1*v1+c1;
const float squareDist2 = num1*num1/(a1*a1+b1*b1);
// 带权重误差
const float chiSquare2 = squareDist2*invSigmaSquare;
// 误差大于阈值就说明这个点是Outlier
if(chiSquare2>th)
bIn = false;
else
score += thScore - chiSquare2;
// Step 2.5 保存结果
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
// 返回评分
return score;
}
/**
* @brief 计算基础矩阵,假设场景为非平面情况下通过前两帧求取Fundamental矩阵,得到该模型的评分
* Step 1 将当前帧和参考帧中的特征点坐标进行归一化
* Step 2 选择8个归一化之后的点对进行迭代
* Step 3 八点法计算基础矩阵矩阵
* Step 4 利用重投影误差为当次RANSAC的结果评分
* Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记
*
* @param[in & out] vbMatchesInliers 标记是否是外点
* @param[in & out] score 计算基础矩阵得分
* @param[in & out] F21 从特征点1到2的基础矩阵
*/
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
// 计算基础矩阵,其过程和上面的计算单应矩阵的过程十分相似.
// Number of putative matches
// 匹配的特征点对总数
// const int N = vbMatchesInliers.size(); // !源代码出错!请使用下面代替
const int N = mvMatches12.size();
// Normalize coordinates
// Step 1 将当前帧和参考帧中的特征点坐标进行归一化,主要是平移和尺度变换
// 具体来说,就是将mvKeys1和mvKey2归一化到均值为0,一阶绝对矩为1,归一化矩阵分别为T1、T2
// 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值
// 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标
vector<cv::Point2f> vPn1, vPn2;
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
// ! 注意这里取的是归一化矩阵T2的转置,因为基础矩阵的定义和单应矩阵不同,两者去归一化的计算也不相同
cv::Mat T2t = T2.t();
// Best Results variables
//最优结果
score = 0.0;
vbMatchesInliers = vector<bool>(N,false);
// Iteration variables
// 某次迭代中,参考帧的特征点坐标
vector<cv::Point2f> vPn1i(8);
// 某次迭代中,当前帧的特征点坐标
vector<cv::Point2f> vPn2i(8);
// 某次迭代中,计算的基础矩阵
cv::Mat F21i;
// 每次RANSAC记录的Inliers与得分
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
// 下面进行每次的RANSAC迭代
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
// Step 2 选择8个归一化之后的点对进行迭代
for(int j=0; j<8; j++)
{
int idx = mvSets[it][j];
// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标
// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引,然后读取其归一化之后的特征点坐标
vPn1i[j] = vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引
vPn2i[j] = vPn2[mvMatches12[idx].second]; //second存储在参考帧1中的特征点索引
}
// Step 3 八点法计算基础矩阵
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
// 基础矩阵约束:p2^t*F21*p1 = 0,其中p1,p2 为齐次化特征点坐标
// 特征点归一化:vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2
// 根据基础矩阵约束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0
// 进一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0
F21i = T2t*Fn*T1;
// Step 4 利用重投影误差为当次RANSAC的结果评分
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
// Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记
if(currentScore>score)
{
//如果当前的结果得分更高,那么就更新最优计算结果
F21 = F21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
}
四、结语
通过前一章篇客,以及这篇博客,了解了Homography矩阵 ,Fundamental矩阵,的求解过程。但是还不够具体,为什么呢。在Initializer::ComputeH21() 或者 Initializer::ComputeF21() 函数中,都调用了比较重要的一个求解函数,那就是 cv::SVDecomp(), 也就是SVD奇异值分解,即 U ∗ Σ ∗ V T U∗Σ∗V^T U∗Σ∗VT。 这才是求解 Homography,Fundamental矩阵的核心,在后面的博客,我们会进行详细的推导。
本文内容来自计算机视觉life ORB-SLAM2 课程课件