一、地图点初始化

二、重新记录特征点的匹配关系

1、构建旋转直方图

1.1、在半径窗口内搜索当前帧F2中所有的候选匹配特征点GetFeaturesInArea

1.2、表示一个图像像素相当于多少个图像网格列和行

1.4、遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里

1.5、根据阈值和角度投票剔除误匹配

1.6、计算匹配点旋转角度差所在的直方图

1.7、根据方向剔除误匹配的点

1.8、将最后通过筛选匹配好的特征点进行保存

三、在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵

构造线程来计算H矩阵及其得分

具体步骤

1、将当前帧和参考帧的特征点坐标进行归一化（对应函数Initializer::Normalize）

为什么要归一化

预先对图像坐标进行归一化有以下好处：

归一化具体操作

2、选择8个归一化的点进行迭代

3、利用生成的8个归一化特征点对, 调用函数 Initializer::ComputeH21() 使用八点法计算单应矩阵

4、利用重投影误差为当次RANSAC的结果评分

5、更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记

计算基础矩阵，假设场景为非平面情况下通过前两帧求取Fundamental矩阵，得到该模型的评分

八点法计算基础矩阵

SVD

SVD分解结果

F矩阵秩为2

使用opencv提供的进行奇异值分解的函数

四、对给定的homography matrix打分,需要使用到卡方检验的知识

卡方检验

为什么要引用卡方检验?

卡方分布用途

卡方分布假设检验步骤

决策原则

五、从基础矩阵F中求解位姿R，t及三维点

六、用H矩阵恢复R, t和三维点

流程:

一、地图点初始化

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))
// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vectorif(nMinCellX>=FRAME_GRID_COLS)return vIndices;// 计算圆所在的右边界网格列索引const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));// 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vectorif(nMaxCellX<0)return vIndices;//后面的操作也都是类似的，计算出这个圆上下边界所在的网格行的idconst 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是当前遍历到的关键帧的特征点idconst 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){// indexint 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^{T}$ 是V的转置 D是一个8 x 9 对角矩阵，除了对角线其他元素均为0，对角线元素称为奇异值，一般来说奇异值是按照从大到小的顺序降序排列。因为每个奇异值都是一个残差项，因此最后一个奇异值最小，其含义就是最优的残差。因此其对应的奇异值向量就是最优值，即最优解。

$V^{T}$ 中的每个列向量对应着D中的每个奇异值，最小二乘最优解就是对应的第9个列向量，也就是基础矩阵F的元素。这里我们先记做 Fpre，因为这个还不是最终的F

F矩阵秩为2

基础矩阵 F 有个很重要的性质，就是秩为2，可以进一步约束求解准确的F ，上面的方法使用对应的第9个列向量构造的Fpre 秩通常不为2，我们可以继续进行SVD分解。

其最小奇异值人为置为0，这样F矩阵秩为2

此时的F就是最终得到的基础矩阵。

使用opencv提供的进行奇异值分解的函数

 cv::SVDecomp(A,							//输入，待进行奇异值分解的矩阵w,							//输出，奇异值矩阵u,							//输出，矩阵Uvt,						//输出，矩阵V^Tcv::SVD::MODIFY_A | 		//输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存cv::SVD::FULL_UV);		//FULL_UV=把U和VT补充成单位正交方阵
// 返回最小奇异值所对应的右奇异向量// 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8return vt.row(8).reshape(0, 			//转换后的通道数，这里设置为0表示是与前面相同3); 			//转换后的行数,对应V的最后一列
}cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //归一化后的点, in reference frameconst 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个奇异值设置为0w.at<float>(2)=0; // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 return  u*cv::Mat::diag(w)*vt;
}

四、对给定的homography matrix打分,需要使用到卡方检验的知识

卡方检验

为什么要引用卡方检验?

以特定概率分布为某种情况建模时，事物长期结果较为稳定，能够清晰进行把握。比如抛硬币实验。但是期望与事实存在差异怎么办？偏差是正常的小幅度波动？还是建模错误？此时，利用卡方分布分析结果，排除可疑结果。

简单来说：当事实与期望不符合情况下使用卡方分布进行检验，看是否系统出了问题，还是属于正常波动

卡方分布用途

检查实际结果与期望结果之间何时存在显著差异。

1、检验拟合程度：也就是说可以检验一组给定数据与指定分布的吻合程度。如：用它检验抽奖机收益的观察频数与我们所期望的吻合程度。

2、检验两个变量的独立性：通过这个方法检查变量之间是否存在某种关系。

卡方分布假设检验步骤

1、确定要进行检验的假设（H0）及其备择假设H1.

2、求出期望E.

3、确定用于做决策的拒绝域（右尾）.

4、根据自由度和显著性水平查询检验统计量临界值.

5、查看检验统计量是否在拒绝域内.

6、做出决策.

根据自由度和显著性水平查询检验统计量临界值

自由度的影响

自由度:用于计算检验统计量的独立变量的数目。

当自由度等于1或者2时：卡方分布先高后低的平滑曲线，检验统计量等于较小值的概率远远大于较大值的概率，即观察频数有可能接近期望频数。

当自由度大于2时：卡方分布先低后高再低，其外形沿着正向扭曲，但当自由度很大时，图形接近正态分布。

自由度的计算，对于单行或单列：自由度 = 组数-限制数对于表格类：自由度 = (行数 - 1) * (列数 - 1)

决策原则

如果位于拒绝域内我们拒绝原假设H0，接受H1。

如果不在拒绝域内我们接受原假设H0，拒绝H1

检验统计量38.272 > 9.49 位于拒绝域内于是拒绝原假设：抽奖机每局收益如何概率分布，也就是说抽奖机被人动了手脚

检验统计量拒绝域内外判定：

1、求出检验统计量a

2、通过自由度和显著性水平查到拒绝域临界值b

3、a>b则位于拒绝域内，反之，位于拒绝域外。

 const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);const float invz1 = 1.0/z1;if(!bStereo1){// 单目情况下float u1 = fx1*x1*invz1+cx1;float v1 = fy1*y1*invz1+cy1;float errX1 = u1 - kp1.pt.x;float errY1 = v1 - kp1.pt.y;// 假设测量有一个像素的偏差，2自由度卡方检验阈值是5.991if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)continue;}else{// 双目情况float u1 = fx1*x1*invz1+cx1;// 根据视差公式计算假想的右目坐标float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;     float v1 = fy1*y1*invz1+cy1;float errX1 = u1 - kp1.pt.x;float errY1 = v1 - kp1.pt.y;float errX1_r = u1_r - kp1_ur;// 自由度为3，卡方检验阈值是7.8if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)continue;}

五、从基础矩阵F中求解位姿R，t及三维点

F分解出E，E有四组解，选择计算的有效三维点（在摄像头前方、投影误差小于阈值、视差角大于阈值）最多的作为最优的解

一些重要参数

 @param[in] vbMatchesInliers          匹配好的特征点对的Inliers标记
* @param[in] F21                       从参考帧到当前帧的基础矩阵
* @param[in] K                         相机的内参数矩阵
* @param[in & out] R21                 计算好的相机从参考帧到当前帧的旋转
* @param[in & out] t21                 计算好的相机从参考帧到当前帧的平移
* @param[in & out] vP3D                三角化测量之后的特征点的空间坐标
* @param[in & out] vbTriangulated      特征点三角化成功的标志
* @param[in] minParallax               认为三角化有效的最小视差角
* @param[in] minTriangulated           最小三角化点数量
* @return true                         成功初始化
* @return false                        初始化失败

1、统计有效匹配点个数，并用 N 表示

2、根据基础矩阵和相机的内参数矩阵计算本质矩阵

定义本质矩阵分解结果，形成四组解,分别是： (R1, t) (R1, -t) (R2, t) (R2, -t)

3、从本质矩阵求解两个R解和两个t解，共四组解

4、分别验证求解的4种R和t的组合，选出最佳组合

六、用H矩阵恢复R, t和三维点

H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition ，代码使用了Faugeras SVD-based decomposition算法。

一些重要参数

* @param[in] vbMatchesInliers          匹配点对的内点标记
* @param[in] H21                       从参考帧到当前帧的单应矩阵
* @param[in] K                         相机的内参数矩阵
* @param[in & out] R21                 计算出来的相机旋转
* @param[in & out] t21                 计算出来的相机平移
* @param[in & out] vP3D                世界坐标系下，三角化测量特征点对之后得到的特征点的空间坐标
* @param[in & out] vbTriangulated      特征点是否成功三角化的标记
* @param[in] minParallax               对特征点的三角化测量中，认为其测量有效时需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
* @param[in] minTriangulated           为了进行运动恢复，所需要的最少的三角化测量成功的点个数
* @return true                         单应矩阵成功计算出位姿和三维点
* @return false                        初始化失败

坐标