网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。
一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！
一、地图点初始化
https://blog.csdn.net/m0_58173801/article/details/119891999?utm_source=app&app_version=4.14.1
在看代码之前可以先看看我前面2D-2D对极几何的介绍，里面详细的说明了本质矩阵E和基础矩阵F的具体求解步骤。
初始换函数（Initialize）:通过两帧匹配关系恢复帧间运动，并计算地图点的位置。
为什么要初始化
因为刚开始没有地图点和初始位姿，用两帧匹配好的特征点三角化得到很多个三维点，用三维点做地图来跟踪下一帧。尺度归一化，初始化为场景的平均深度。
先计算基础矩阵和单应性矩阵，选取最佳的来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点。
**\* Step 1 重新记录特征点对的匹配关系
\* Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
\* Step 3 计算fundamental 矩阵 和homography 矩阵，为了加速分别开了线程计算
\* Step 4 计算得分比例来判断选取哪个模型来求位姿R,t**
一些重要的参数
**\* @param[in] ReferenceFrame 参考帧
\* @param[in] sigma 测量误差
\* @param[in] iterations RANSAC迭代次数
\* @param[in] CurrentFrame 当前帧，也就是SLAM意义上的第二帧
\* @param[in] vMatches12 当前帧（2）和参考帧（1）图像中特征点的匹配关系
\* vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
\* 没有匹配关系的话，vMatches12[i]值为 -1
\* @param[in & out] R21 相机从参考帧到当前帧的旋转
\* @param[in & out] t21 相机从参考帧到当前帧的平移
\* @param[in & out] vP3D 三角化测量之后的三维地图点
\* @param[in & out] vbTriangulated 标记三角化点是否有效，有效为true
\* @return true 该帧可以成功初始化，返回true
\* @return false 该帧不满足初始化条件，返回false**
二、重新记录特征点的匹配关系
单目初始化中用于参考帧和当前帧的特征点匹配
**\* 步骤
\* Step 1 构建旋转直方图
\* Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点
\* Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
\* Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
\* Step 5 计算匹配点旋转角度差所在的直方图
\* Step 6 筛除旋转直方图中“非主流”部分
\* Step 7 将最后通过筛选的匹配好的特征点保存**
一些重要的参数
**\* @param[in] F1 初始化参考帧
\* @param[in] F2 当前帧
\* @param[in & out] vbPrevMatched 本来存储的是参考帧的所有特征点坐标，该函数更新为匹 配好的当前帧的特征点坐标
\* @param[in & out] vnMatches12 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
\* @param[in] windowSize 搜索窗口
\* @return int 返回成功匹配的特征点数目**
1、构建旋转直方图
HISTO_LENGTH = 30
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
// 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量
rotHist[i].reserve(500);
const float factor = HISTO_LENGTH/360.0f;
// 匹配点对距离，注意是按照F2特征点数目分配空间
vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
// 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间
vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
// 遍历帧1中的所有特征点
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
{
cv::KeyPoint kp1 = F1.mvKeysUn[i1];
int level1 = kp1.octave;
// 只使用原始图像上提取的特征点
if(level1>0)
continue;
1.1、在半径窗口内搜索当前帧F2中所有的候选匹配特征点GetFeaturesInArea
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
// 计算特征点x,y坐标落在哪个网格内，网格坐标为posX，posY
// mfGridElementWidthInv=(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
// mfGridElementHeightInv=(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);
1.2、表示一个图像像素相当于多少个图像网格列和行
// 表示一个图像像素相当于多少个图像网格列（宽）
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
// 表示一个图像像素相当于多少个图像网格行（高）
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
1.3、计算半径为r的圆左右上下边界所在网格列和行的ID
首先我们要查找半径为r的圆左侧边界所在网格列坐标。这个地方有点绕，慢慢理解下： (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素（肯定大于1）
mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数，表示每个像素可以均分几个网格列（肯定小于1）
(x-mnMinX-r)，可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数
两者相乘，就是求出那个半径为r的圆的左侧边界在哪个网格列中
保证nMinCellX 结果大于等于0
//计算半径为r的圆左右上下边界所在网格列和行的ID
const int nMinCellX = max(0,(int)floor( (x-mnMinX-r)*mfGridElementWidthInv))
// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vector
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
// 计算圆所在的右边界网格列索引
const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
// 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vector
if(nMaxCellX<0)
return vIndices;
//后面的操作也都是类似的，计算出这个圆上下边界所在的网格行的id
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;
const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
1.4、遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
// 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
const vector<size_t> vCell = mGrid[ix][iy];
// 如果这个网格中没有特征点，那么跳过这个网格继续下一个
if(vCell.empty())
continue;
// 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
// 根据索引先读取这个特征点
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
// 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
// 如果x方向和y方向的距离都在指定的半径之内，存储其index为候选特征点
if(fabs(distx)<r && fabs(disty)<r)
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
1.5、根据阈值 和 角度投票剔除误匹配
匹配距离必须小于设定阈值
if(bestDist1<=TH_LOW)
{
// Step 4.2：第二关筛选：最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
// Step 4.3：记录成功匹配特征点的对应的地图点(来自关键帧)
vpMapPointMatches[bestIdxF]=pMP;
// 这里的realIdxKF是当前遍历到的关键帧的特征点id
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
1.6、计算匹配点旋转角度差所在的直方图
if(mbCheckOrientation)
{
// angle：每个特征点在提取描述子时的旋转主方向角度，如果图像旋转了，这个角度将发生改变
// 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值
float rot = kp.angle-F.mvKeys[bestIdxF].angle;// 该特征点的角度变化值
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);// 将rot分配到bin组, 四舍五入, 其实就是离散到对应的直方图组中
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(bestIdxF); // 直方图统计
}
nmatches++;
1.7、根据方向剔除误匹配的点
if(mbCheckOrientation)
{
// index
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
// 如果特征点的旋转角度变化量属于这三个组，则保留
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
return nmatches;
}
1.8、将最后通过筛选匹配好的特征点进行保存
for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)
if(vnMatches12[i1]>=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
三、在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
共选择 mMaxIterations (默认200) 组 ，mvSets用于保存每次迭代时所使用的向量。
将上面匹配好的特征点重新记录特征点对的匹配关系存储在mvMatches12，是否有匹配存储在mvbMatched1，在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵。而且在计算fundamental 矩阵 和homography 矩阵，为了加速分别开了线程计算。线程计算也是我们前面介绍过的加锁和释放锁，主要目的是为了提高计算速度。计算出来的单应矩阵和基础矩阵的RANSAC评分，这里其实是采用重投影误差来计算的。
构造线程来计算H矩阵及其得分
详细的求解原理已经在前面细讲过了，大家可以看前面连接上面的文章，这里就把最后推导出来的公式拿出来了。
一对点提供两个约束等式，单应矩阵H总共有9个元素，8个自由度（尺度等价性），所以需要4对点提供 8个约束方程就可以求解。
thread方法比较特殊，在传递引用的时候，外层需要用ref来进行引用传递，否则就是浅拷贝
一些重要参数
**FindHomography 该线程的主函数
ref(vbMatchesInliersH), 输出，特征点对的Inlier标记
ref(SH), 输出，计算的单应矩阵的RANSAC评分
ref(H)); 输出，计算的单应矩阵结果**
**@param[in & out] vbMatchesInliers 标记是否是外点
@param[in & out] score 计算单应矩阵的得分
@param[in & out] H21 单应矩阵结果**
具体步骤
计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分
原理参考Multiple view geometry in computer vision P109 算法4.4
* Step 1 将当前帧和参考帧中的特征点坐标进行归一化
* Step 2 选择8个归一化之后的点对进行迭代
* Step 3 八点法计算单应矩阵矩阵
* Step 4 利用重投影误差为当次RANSAC的结果评分
* Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
1、将当前帧和参考帧的特征点坐标进行归一化（对应函数Initializer::Normalize）
原理参考
Multiple view geometry in computer vision P109 算法4.4
Data normalization is an essential step in the DLT algorithm. It must not be considered optional. Data normalization becomes even more important for less well conditioned problems, such as the DLT computation of the fundamental matrix or the trifocal tensor, which will be considered in later chapters
为什么要归一化
Ah=0
矩阵A是利用8点法求基础矩阵的关键，所以Hartey就认为，利用8点法求基础矩阵不稳定的一个主要原因就是原始的图像像点坐标组成的系数矩阵A不好造成的，而造成A不好的原因是像点的齐次坐标各个分量的数量级相差太大。基于这个原因，Hartey提出一种改进的8点法，在应用8点法求基础矩阵之前，先对像点坐标进行归一化处理，即对原始的图像坐标做同向性变换，这样就可以减少噪声的干扰，大大的提高8点法的精度。
预先对图像坐标进行归一化有以下好处：
能够提高运算结果的精度
利用归一化处理后的图像坐标，对任何尺度缩放和原点的选择是不变的。归一化步骤预先为图像坐 标选择了一个标准的坐标系中，消除了坐标变换对结果的影响。
归一化操作分两步进行，首先对每幅图像中的坐标进行平移（每幅图像的平移不同）使图像中匹配的 组成的点集的形心（Centroid）移动到原点；接着对坐标系进行缩放使得各个分量总体上有一样的平均值，各个坐标轴的缩放相同的。
注：使用归一化的坐标虽然能一定程度的消除噪声、错误匹配带来的影响，但还是不够的。
归一化具体操作
一阶矩就是随机变量的期望，二阶矩就是随机变量平方的期望；一阶绝对矩定义为变量与均值绝对值的平均。
将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换 ，具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2 ，这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值 ，归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标
//归一化后的参考帧1和当前帧2中的特征点坐标
vector<cv::Point2f> vPn1, vPn2;
// 记录各自的归一化矩阵
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
//这里求的逆在后面的代码中要用到，辅助进行原始尺度的恢复
cv::Mat T2inv = T2.inv();
2、选择8个归一化的点进行迭代
for(size_t j=0; j<8; j++)
{
//从mvSets中获取当前次迭代的某个特征点对的索引信息
int idx = mvSets[it][j];
// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标
// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标
vPn1i[j] = vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引
vPn2i[j] = vPn2[mvMatches12[idx].second]; //second存储在参考帧1中的特征点索引
}//读取8对特征点的归一化之后的坐标
3、利用生成的8个归一化特征点对, 调用函数 Initializer::ComputeH21() 使用八点法计算单应矩阵
单应矩阵原理：X2=H21*X1，其中X1,X2 为归一化后的特征点
特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2 得到:T2 * mvKeys2 = Hn * T1 * mvKeys1
进一步得到:mvKeys2 = T2.inv * Hn * T1 * mvKeys1
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
H21i = T2inv*Hn*T1;
//然后计算逆
H12i = H21i.inv();
4、利用重投影误差为当次RANSAC的结果评分
RANSAC算法
少数外点会极大影响计算结果的准确度.随着采样数量的增加,外点数量也会同时增加,这是一种系统误差,无法通过增加采样点来解决.
RANSAC(Random sample consensus,随机采样一致性)算法的思路是少量多次重复实验,每次实验仅使用尽可能少的点来计算,并统计本次计算中的内点数.只要尝试次数足够多的话,总会找到一个包含所有内点的解.
RANSAC算法的核心是减少每次迭代所需的采样点数.从原理上来说,计算F矩阵最少只需要7对匹配点,计算H矩阵最少只需要4对匹配点;ORB-SLAM2中为了编程方便,每次迭代使用8对匹配点计算F和H.
currentScore = CheckHomography(H21i, H12i, //输入，单应矩阵的计算结果
vbCurrentInliers, //输出，特征点对的Inliers标记
mSigma); //TODO 测量误差，在Initializer类对象构造的时候，由外部给定的
5、更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
if(currentScore>score)
{
//如果当前的结果得分更高，那么就更新最优计算结果
H21 = H21i.clone();
//保存匹配好的特征点对的Inliers标记
vbMatchesInliers = vbCurrentInliers;
//更新历史最优评分
score = currentScore;
}
计算基础矩阵，假设场景为非平面情况下通过前两帧求取Fundamental矩阵，得到该模型的评分
等式左边两项分别用A, f表示，则有
Af=0
一对点提供一个约束方程，基础矩阵F总共有9个元素，7个自由度（尺度等价性，秩为2），所以8对点 提供8个约束方程就可以求解F。
求解基础矩阵F和求解单应矩阵类似，我们就不一一展开了。
八点法计算基础矩阵
基础矩阵约束：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
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
F21i = T2t*Fn*T1;
SVD
SVD分解结果
假设我们使用8对点求解，A 是 8x9 矩阵，分解后 U 是左奇异向量，它是一个8x8的 正交矩阵， V 是右奇异向量，是一个 9x9 的正交矩阵， 是V的转置 D是一个8 x 9 对角矩阵，除了对角线其他元素均为0，对角线元素称为奇异值，一般来说奇异值是按照 从大到小的顺序降序排列。因为每个奇异值都是一个残差项，因此最后一个奇异值最小，其含义就是最 优的残差。因此其对应的奇异值向量就是最优值，即最优解。
中的每个列向量对应着D中的每个奇异值，最小二乘最优解就是 对应的第9个列向量，也就是基础 矩阵F的元素。这里我们先记做 Fpre，因为这个还不是最终的F
F矩阵秩为2
既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上C C++开发知识点，真正体系化！
由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频，并且后续会持续更新
V^{T}](https://latex.csdn.net/eq?V%5E%7BT%7D) 是V的转置 D是一个8 x 9 对角矩阵，除了对角线其他元素均为0，对角线元素称为奇异值，一般来说奇异值是按照 从大到小的顺序降序排列。因为每个奇异值都是一个残差项，因此最后一个奇异值最小，其含义就是最 优的残差。因此其对应的奇异值向量就是最优值，即最优解。
中的每个列向量对应着D中的每个奇异值，最小二乘最优解就是 对应的第9个列向量，也就是基础 矩阵F的元素。这里我们先记做 Fpre，因为这个还不是最终的F
F矩阵秩为2
[外链图片转存中…(img-fLU27nHE-1715839762243)]
[外链图片转存中…(img-HgC0MCBq-1715839762243)]
既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上C C++开发知识点，真正体系化！
由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频，并且后续会持续更新
1095
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
