ORB-SLAM2学习(原理):Initialize.cc
详细中文源码解读:链接:https://pan.baidu.com/s/1LWfowy5wbUdXamEGE1STcA 提取码:t796
PS:该代码从“计算机视觉life”客服处免费获得,感觉确实挺详细的,就标明一下出处,侵权则删。
文章目录
- ORB-SLAM2学习(原理):Initialize.cc
- 构造初始化器Initializer::Initializer
- 构造函数Initializer::Initialize
- 计算单应矩阵H Initializer::FindHomography和Initializer::ComputeH21
- 计算基础矩阵F Initializer::FindFundamental和Initializer::ComputeF21
- 单应矩阵H打分 Initializer::CheckHomography
- 基础矩阵F打分 Initializer::CheckFundamental
- 单应矩阵H恢复平面 Initializer::ReconstructH
- 基础矩阵F恢复平面 Initializer::ReconstructH
- 归一化 Initializer::Normalize
- 筛选中合格的三维点 Initializer::CheckRT
- 原理补充
构造初始化器Initializer::Initializer
出现位置在Tracking.cc中,作用是读取参考帧的相机模型, 内参, 去畸变的特征点等传入参数。
// 由当前帧构造初始器 sigma:1.0 iterations:200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
/**
* @brief 根据参考帧构造初始化器
*
* @param[in] ReferenceFrame 参考帧
* @param[in] sigma 测量误差
* @param[in] iterations RANSAC迭代次数
*/
Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
{
//从参考帧中获取相机的内参数矩阵
mK = ReferenceFrame.mK.clone();
// 从参考帧中获取去畸变后的特征点
mvKeys1 = ReferenceFrame.mvKeysUn;
//获取估计误差
mSigma = sigma;
mSigma2 = sigma*sigma;
//最大迭代次数
mMaxIterations = iterations;
}
构造函数Initializer::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)
step 1:重新记录特征点对的匹配关系
{
// Fill structures with current keypoints and matches with reference frame
// Reference Frame: 1, Current Frame: 2
//获取当前帧的去畸变之后的特征点
mvKeys2 = CurrentFrame.mvKeysUn;
// mvMatches12记录匹配上的特征点对,记录的是帧2在帧1的匹配索引
mvMatches12.clear();
// 预分配空间,大小和关键点数目一致mvKeys2.size()
mvMatches12.reserve(mvKeys2.size());
// 记录参考帧1中的每个特征点是否有匹配的特征点
// 这个成员变量后面没有用到,后面只关心匹配上的特征点
mvbMatched1.resize(mvKeys1.size());
// Step 1 重新记录特征点对的匹配关系存储在mvMatches12,是否有匹配存储在mvbMatched1
// 将vMatches12(有冗余) 转化为 mvMatches12(只记录了匹配关系)
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
//vMatches12[i]解释:i表示帧1中关键点的索引值,vMatches12[i]的值为帧2的关键点索引值
//没有匹配关系的话,vMatches12[i]值为 -1
if(vMatches12[i]>=0)
{
//mvMatches12 中只记录有匹配关系的特征点对的索引值
//i表示帧1中关键点的索引值,vMatches12[i]的值为帧2的关键点索引值
mvMatches12.push_back(make_pair(i,vMatches12[i]));
//标记参考帧1中的这个特征点有匹配关系
mvbMatched1[i]=true;
}
else
//标记参考帧1中的这个特征点没有匹配关系
mvbMatched1[i]=false;
}
// 有匹配的特征点的对数
const int N = mvMatches12.size();
// Indices for minimum set selection
// 新建一个容器vAllIndices存储特征点索引,并预分配空间
vector<size_t> vAllIndices;
vAllIndices.reserve(N);
//在RANSAC的某次迭代中,还可以被抽取来作为数据样本的特征点对的索引,所以这里起的名字叫做可用的索引
vector<size_t> vAvailableIndices;
//初始化所有特征点对的索引,索引值0到N-1
for(int i=0; i<N; i++)
{
vAllIndices.push_back(i);
}
mvbMatched1[i]:mvMatches12 中只记录有匹配关系的特征点对的索引值,i表示帧1中关键点的索引值,vMatches12[i]的值为帧2的关键点索引值.
Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组,用于估计H矩阵和F矩阵
// Generate sets of 8 points for each RANSAC iteration
// Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组,用于估计H矩阵和F矩阵
// 共选择 mMaxIterations (默认200) 组
//mvSets用于保存每次迭代时所使用的向量
mvSets = vector< vector<size_t> >(mMaxIterations, //最大的RANSAC迭代次数
vector<size_t>(8,0)); //这个则是第二维元素的初始值,也就是第一维。这里其实也是一个第一维的构造函数,第一维vector有8项,每项的初始值为0.
//用于进行随机数据样本采样,设置随机数种子
DUtils::Random::SeedRandOnce(0);
//开始每一次的迭代
for(int it=0; it<mMaxIterations; it++)
{
//迭代开始的时候,所有的点都是可用的
vAvailableIndices = vAllIndices;
// Select a minimum set
//选择最小的数据样本集,使用八点法求,所以这里就循环了八次
for(size_t j=0; j<8; j++)
{
// 随机产生一对点的id,范围从0到N-1
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
// idx表示哪一个索引对应的特征点对被选中
int idx = vAvailableIndices[randi];
//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中
mvSets[it][j] = idx;
// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,
// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素
// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}//依次提取出8个特征点对
}//迭代mMaxIterations次,选取各自迭代时需要用到的最小数据集
RANSA算法参考RANSAC算法原理
Step 3 计算fundamental 矩阵 和homography 矩阵,为了加速分别开了线程计算
// Launch threads to compute in parallel a fundamental matrix and a homography
// Step 3 计算fundamental 矩阵 和homography 矩阵,为了加速分别开了线程计算
//这两个变量用于标记在H和F的计算中哪些特征点对被认为是Inlier
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
//计算出来的单应矩阵和基础矩阵的RANSAC评分,这里其实是采用重投影误差来计算的
float SH, SF; //score for H and F
//这两个是经过RANSAC算法后计算出来的单应矩阵和基础矩阵
cv::Mat H, F;
// 构造线程来计算H矩阵及其得分
// thread方法比较特殊,在传递引用的时候,外层需要用ref来进行引用传递,否则就是浅拷贝
thread threadH(&Initializer::FindHomography, //该线程的主函数
this, //由于主函数为类的成员函数,所以第一个参数就应该是当前对象的this指针
ref(vbMatchesInliersH), //输出,特征点对的Inlier标记
ref(SH), //输出,计算的单应矩阵的RANSAC评分
ref(H)); //输出,计算的单应矩阵结果
// 计算fundamental matrix并打分,参数定义和H是一样的,这里不再赘述
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
//等待两个计算线程结束
threadH.join();
threadF.join();
Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
// Compute ratio of scores
// Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
//通过这个规则来判断谁的评分占比更多一些,注意不是简单的比较绝对评分大小,而是看评分的占比
float RH = SH/(SH+SF); //RH=Ratio of Homography
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
// 注意这里更倾向于用H矩阵恢复位姿。如果单应矩阵的评分占比达到了0.4以上,则从单应矩阵恢复运动,否则从基础矩阵恢复运动
if(RH>0.40)
//更偏向于平面,此时从单应矩阵恢复,函数ReconstructH返回bool型结果
return ReconstructH(vbMatchesInliersH, //输入,匹配成功的特征点对Inliers标记
H, //输入,前面RANSAC计算后的单应矩阵
mK, //输入,相机的内参数矩阵
R21,t21, //输出,计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换
vP3D, //特征点对经过三角测量之后的空间坐标,也就是地图点
vbTriangulated, //特征点对是否成功三角化的标记
1.0, //这个对应的形参为minParallax,即认为某对特征点的三角化测量中,认为其测量有效时
//需要满足的最小视差角(如果视差角过小则会引起非常大的观测误差),单位是角度
50); //为了进行运动恢复,所需要的最少的三角化测量成功的点个数
else //if(pF_HF>0.6)
// 更偏向于非平面,从基础矩阵恢复
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
//一般地程序不应该执行到这里,如果执行到这里说明程序跑飞了
return false;
}
计算单应矩阵H Initializer::FindHomography和Initializer::ComputeH21
/**
* @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)
计算基础矩阵F Initializer::FindFundamental和Initializer::ComputeF21
/**
* @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)
单应矩阵H打分 Initializer::CheckHomography
/**
* @brief 对给定的homography matrix打分, 需要使用到卡方检验的知识
*
* @param[in] H21 从参考帧到当前帧的单应矩阵
* @param[in] H12 从当前帧到参考帧的单应矩阵
* @param[in] vbMatchesInliers 匹配好的特征点对的Inliers标记
* @param[in] sigma 方差,默认为1
* @return float 返回得分
*/
float Initializer::CheckHomography(
基础矩阵F打分 Initializer::CheckFundamental
/**
* @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) //方差
单应矩阵H恢复平面 Initializer::ReconstructH
/**
* @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)
基础矩阵F恢复平面 Initializer::ReconstructH
/**
* @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)
{
归一化 Initializer::Normalize
/**
* @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;
}
筛选中合格的三维点 Initializer::CheckRT
原理补充
本质矩阵E原理、如何求解、分解R,t
参考链接:
原理及求解视觉slam十四讲P143-P148
一步步带你看懂orbslam2源码–单目初始化(三)
秋招准备(立体视觉基础——基础矩阵与本质矩阵)分解R,t
问:“我们根据线性方程求出的E可能不满足E的内在性质——奇异值可能不为([\sigma ,\sigma ,0]^T)的形式。因此,我们会刻意将(\Sigma)矩阵调整为上面的样子。”
自由度
视觉SLAM中,本质矩阵、基础矩阵、单应性矩阵自由度和秩?
多视图几何总结——基础矩阵、本质矩阵和单应矩阵的自由度分析
视觉SLAM 第7讲 本质矩阵 基础矩阵 单应矩阵 知识点/证明/理解/秩/自由度本质矩阵的尺度等价性:
基础矩阵F原理、R,t分解
单应矩阵H原理、分解
视觉slam十四讲P148-P151
一步步带你看懂orbslam2源码–单应矩阵/基础矩阵,求解R,t(四)
三角测量
卡方检验
https://blog.csdn.net/weixin_44943764/article/details/127609973
H恢复平面
参考这篇论文Motion and Structure from Motion in a Piecewise Planar Environment
3月12日 对极几何,本征矩阵,基础矩阵,F/E矩阵计算,恢复旋转与平移,三角化视图重建