不同类型的相机初始化不同，对于双目相机通过左右目图像立体匹配得到特征点对应三维空间坐标；RGB-D相机每个点的深度通过相机物理特性求得，因此也很容易求出特征点的三维空间坐标，双目和RGB-D相机在第一帧就可以初始化；单目相机在第一帧得不到特征点的三维坐标，需要通过三角测量的方法在两帧求得特征点的深度值。
2d-2d: 对极几何 3d-2d： PnP 3d-3d： ICP，这部分参考十四讲ch7
在mono_kitti.cc的第三步TrackMonocular函数中核心地方在于GrabImageMonocular函数求得相机位姿，该函数分为三个部分：彩色图像转为灰度图像、构造Frame、Track跟踪,前两部分之前已经学习过，这篇文章是对Track函数的介绍，
这个函数分为估计相机运动和跟踪局部地图
1.地图初始化，单目初始化和 双目、RGB-D相机不同
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
//双目RGBD相机的初始化共用一个函数
StereoInitialization();
else
//单目初始化
MonocularInitialization();
//更新帧绘制器中存储的最新状态
mpFrameDrawer->Update(this);
//这个状态量在上面的初始化函数中被更新
if(mState!=OK)
return;
}先看单目相机的初始化函数 MonocularInitialization
第一帧没用被初始化，执行
if(!mpInitializer)
{
// Set Reference Frame
// 单目初始帧的特征点数必须大于100
if(mCurrentFrame.mvKeys.size()>100)
{
// 初始化需要两帧，分别是mInitialFrame，mCurrentFrame
mInitialFrame = Frame(mCurrentFrame);
// 用当前帧更新上一帧
mLastFrame = Frame(mCurrentFrame);
// mvbPrevMatched 记录"上一帧"所有特征点
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
// 删除前判断一下，来避免出现段错误。不过在这里是多余的判断
// 不过在这里是多余的判断，因为前面已经判断过了
if(mpInitializer)
delete mpInitializer;
// 由当前帧构造初始器 sigma:1.0 iterations:200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
// 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的id
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
}第二帧不需要初始化，执行后面一个大的else{...}
先是对特征点数量进行判断，小于100就退出
// Step 2 如果当前帧特征点数太少（不超过100），则重新构造初始器
// NOTICE 只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
if((int)mCurrentFrame.mvKeys.size()<=100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}然后对这两帧进行匹配，使用了 SearchForInitialization函数，返回值是nmatches
// Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对
ORBmatcher matcher(
0.9, //最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7
true); //检查特征点的方向
// 对 mInitialFrame,mCurrentFrame 进行特征点匹配
// mvbPrevMatched为参考帧的特征点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标
// mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
int nmatches = matcher.SearchForInitialization(
mInitialFrame,mCurrentFrame, //初始化时的参考帧和当前帧
mvbPrevMatched, //在初始化参考帧中提取得到的特征点
mvIniMatches, //保存匹配关系
100); //搜索窗口大小SearchForInitialization函数：
/**
* @brief 单目初始化中用于参考帧和当前帧的特征点匹配
* 步骤
* 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 返回成功匹配的特征点数目
*/
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
然后对nmatches进行判断 ，如果匹配点不符合要求，需要重新初始化
//Step 4 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
cv::Mat Rcw; // 旋转矩阵
cv::Mat tcw; // 平移向量
vector<bool> vbTriangulated; // 判断特征点是否被三角化匹配结果符合要求，通过单应矩阵或矩阵矩阵模型（对极几何）计算两帧之间的相对运动，也就是位姿变换矩阵，使用 Initialize 函数判断该帧是否可以成功初始化，这个函数很重要！！！
// Step 5 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
if(mpInitializer->Initialize(
mCurrentFrame, //当前帧
mvIniMatches, //当前帧和参考帧的特征点的匹配关系
Rcw, tcw, //初始化得到的相机的位姿
mvIniP3D, //进行三角化得到的空间点集合
vbTriangulated)) //以及对应于mvIniMatches来讲,其中哪些点被三角化了
{在 Initialize 函数中计算单应矩阵和基础矩阵，选取最佳的恢复出两帧之间的相对位姿，并进行三角化得到初始化地图点。
/**
* @brief 计算基础矩阵和单应性矩阵，选取最佳的来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点
* Step 1 重新记录特征点对的匹配关系
* Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
* Step 3 计算fundamental 矩阵 和homography 矩阵，为了加速分别开了线程计算
* Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
*
*
* @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
*/
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)计算单应矩阵函数 FindHomography
/**
* @brief 计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分
* 原理参考Multiple view geometry in computer vision P109 算法4.4
* Step 1 将当前帧和参考帧中的特征点坐标进行归一化
* Step 2 选择8个归一化之后的点对进行迭代
* Step 3 八点法计算单应矩阵矩阵
* Step 4 利用重投影误差为当次RANSAC的结果评分
* Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
*
* @param[in & out] vbMatchesInliers 标记是否是外点
* @param[in & out] score 计算单应矩阵的得分
* @param[in & out] H21 单应矩阵结果
*/
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)计算基础矩阵函数 FindFundamental
/**
* @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)step1进行归一化操作
将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换。具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2。这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值。归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标
归一化的原因
* 在相似变换之后(点在不同的坐标系下),他们的单应性矩阵是不相同的
* 如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化
* 我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一
* 对同一幅图像的坐标进行相同的变换,不同图像进行不同变换
* 缩放尺度是为了让噪声对于图像的影响在一个数量级上
/**
* @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) //将特征点归一化的矩阵
Step 3 八点法计算单应矩阵
利用生成的8个归一化特征点对, 调用函数 Initializer::ComputeH21() 使用八点法计算单应矩阵
/**
* @brief 用DLT方法求解单应矩阵H
* 这里最少用4对点就能够求出来，不过这里为了统一还是使用了8对点求最小二乘解
*
* @param[in] vP1 参考帧中归一化后的特征点
* @param[in] vP2 当前帧中归一化后的特征点
* @return cv::Mat 计算的单应矩阵H
*/
cv::Mat Initializer::ComputeH21(
const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
注意：使用ComputeH21函数求的是归一化后两帧特征点的单应矩阵
Step 4 利用重投影误差为当次RANSAC的结果评分
调用CheckHomography函数
/**
* @brief 对给定的homography matrix打分,需要使用到卡方检验的知识
*
* @param[in] H21 从参考帧到当前帧的单应矩阵
* @param[in] H12 从当前帧到参考帧的单应矩阵
* @param[in] vbMatchesInliers 匹配好的特征点对的Inliers标记
* @param[in] sigma 方差，默认为1
* @return float 返回得分
*/
float Initializer::CheckHomography(
const cv::Mat &H21, //从参考帧到当前帧的单应矩阵
const cv::Mat &H12, //从当前帧到参考帧的单应矩阵
vector<bool> &vbMatchesInliers, //匹配好的特征点对的Inliers标记
float sigma) //估计误差计算基础矩阵函数 FindHomography
/**
* @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)
归一化和单应矩阵一样，在计算基础矩阵的时候不同于单应矩阵。
/**
* @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基础矩阵有9个未知数，自由度是7（尺度等价性、秩为2）
注意：使用ComputeF21函数求的是归一化后两帧特征点的基础矩阵，
利用重投影误差为当次RANSAC的结果评分
/**
* @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) //方差Initialize函数最后是从单应矩阵或者基础矩阵中求解位姿R、t、三维点
/**
* @brief 用H矩阵恢复R, t和三维点
* H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition
* 代码使用了Faugeras SVD-based decomposition算法，参考文献
* Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988
*
* @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 初始化失败
*/
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)从基础矩阵中求解位姿R、t、三维点
/**
* @brief 从基础矩阵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 初始化失败
*/
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
初始化成功后删除无法进行三角化的匹配点
// Step 6 初始化成功后，删除那些无法进行三角化的匹配点
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}将初始化成功的第一帧作为世界坐标系
// Step 7 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到相机坐标系的变换矩阵
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose(Tcw);创建初始化地图点
// Step 8 创建初始化地图点MapPoints
// Initialize函数会得到mvIniP3D，
// mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
// CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
CreateInitialMapMonocular();
}2.初始化完成后进入相机跟踪，主要由三种跟踪方式
3.相机跟踪成功后对局部地图进行跟踪，这里需要特征匹配得到更多地图点，也要优化相机位姿
4.更新显示线程中的图像、特征点、地图点等信息
5.跟踪成功，更新恒速运动模型
6.清除观测不到的地图点
7.清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
8..对当前帧进行判断是否可以作为关键帧
9.删除BA优化中的外点
10.如果初始化和重定位都失败，需要reset
6604
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
