ORB_SLAM2逐行解析(3)
mpTracker = new Tracking(this, //现在还不是很明白为什么这里还需要一个this指针 TODO
mpVocabulary, //字典
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpMap, //地图
mpKeyFrameDatabase, //关键帧地图
strSettingsFile, //设置文件路径
mSensor); //传感器类型iomanip// Step 1 从配置文件中加载相机参数
cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
float fx = fSettings["Camera.fx"];
float fy = fSettings["Camera.fy"];
float cx = fSettings["Camera.cx"];
float cy = fSettings["Camera.cy"];cv::FileStorage::WRITE
int main() {
cv::FileStorage fwrite("./settings.yaml",cv::FileStorage::WRITE);
int fx=100;
int fy=100;
fwrite <<"fx"<%YAML:1.0
---
fx: 100
fy: 100
构造相机内参矩阵
// |fx 0 cx|
// K = |0 fy cy|
// |0 0 1 |
//构造相机内参矩阵
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
K.at(0,0) = fx;
K.at(1,1) = fy;
K.at(0,2) = cx;
K.at(1,2) = cy;
K.copyTo(mK); 图像矫正系数
// 图像矫正系数
// [k1 k2 p1 p2 k3]
cv::Mat DistCoef(4,1,CV_32F);
DistCoef.at(0) = fSettings["Camera.k1"];
DistCoef.at(1) = fSettings["Camera.k2"];
DistCoef.at(2) = fSettings["Camera.p1"];
DistCoef.at(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
//有些相机的畸变系数中会没有k3项
if(k3!=0)
{
DistCoef.resize(5);
DistCoef.at(4) = k3;
}
DistCoef.copyTo(mDistCoef); 双目摄像头baseline * fx 50
// 双目摄像头baseline * fx 50
mbf = fSettings["Camera.bf"];
float fps = fSettings["Camera.fps"];
if(fps==0)
fps=30;
// Max/Min Frames to insert keyframes and to check relocalisation
mMinFrames = 0;
mMaxFrames = fps;// Step 2 加载ORB特征点有关的参数,并新建特征点提取器
// 每一帧提取的特征点数 1000
int nFeatures = fSettings["ORBextractor.nFeatures"];
// 图像建立金字塔时的变化尺度 1.2
float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
// 尺度金字塔的层数 8
int nLevels = fSettings["ORBextractor.nLevels"];
// 提取fast特征点的默认阈值 20
int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
// 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
int fMinThFAST = fSettings["ORBextractor.minThFAST"];// tracking过程都会用到mpORBextractorLeft作为特征点提取器
mpORBextractorLeft = new ORBextractor(
nFeatures, //参数的含义还是看上面的注释吧
fScaleFactor,
nLevels,
fIniThFAST,
fMinThFAST);ORBextractor::ORBextractor(int _nfeatures, //指定要提取的特征点数目
float _scaleFactor, //指定图像金字塔的缩放系数
int _nlevels, //指定图像金字塔的层数
int _iniThFAST, //指定初始的FAST特征点提取参数，可以提取出最明显的角点
int _minThFAST): //如果初始阈值没有检测到角点，降低到这个阈值提取出弱一点的角点
nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
iniThFAST(_iniThFAST), minThFAST(_minThFAST)//设置这些参数存储每层图像缩放系数的vector调整为符合图层数目的大小
//存储每层图像缩放系数的vector调整为符合图层数目的大小
mvScaleFactor.resize(nlevels);
//存储这个sigma^2，其实就是每层图像相对初始图像缩放因子的平方
mvLevelSigma2.resize(nlevels);
//对于初始图像，这两个参数都是1
mvScaleFactor[0]=1.0f;
mvLevelSigma2[0]=1.0f;逐层计算图像金字塔中图像相当于初始图像的缩放系数
//然后逐层计算图像金字塔中图像相当于初始图像的缩放系数
for(int i=1; i
调整图像金字塔vector以使得其符合设定的图像层数
//调整图像金字塔vector以使得其符合设定的图像层数
mvImagePyramid.resize(nlevels);
//每层需要提取出来的特征点个数，这个向量也要根据图像金字塔设定的层数进行调整
mnFeaturesPerLevel.resize(nlevels);
第0层图像应该分配的特征点数量
//图片降采样缩放系数的倒数
float factor = 1.0f / scaleFactor;
//第0层图像应该分配的特征点数量
float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
逐层计算要分配的特征点个数
//用于在特征点个数分配的，特征点的累计计数清空
int sumFeatures = 0;
//开始逐层计算要分配的特征点个数，顶层图像除外（看循环后面）
for( int level = 0; level < nlevels-1; level++ )
{
//分配 cvRound : 返回个参数最接近的整数值
mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
//累计
sumFeatures += mnFeaturesPerLevel[level];
//乘系数
nDesiredFeaturesPerScale *= factor;
}
//由于前面的特征点个数取整操作，可能会导致剩余一些特征点个数没有被分配，所以这里就将这个余出来的特征点分配到最高的图层中
mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);max()函数是算法标头的库函数，用于从给定的两个值中查找最大值，它接受两个值并返回最大值，如果两个值相同，则返回第一个值。
//成员变量pattern的长度，也就是点的个数，这里的512表示512个点（上面的数组中是存储的坐标所以是256*2*2）
const int npoints = 512;
//获取用于计算BRIEF描述子的随机采样点点集头指针
//注意到pattern0数据类型为Points*,bit_pattern_31_是int[]型，所以这里需要进行强制类型转换
const Point* pattern0 = (const Point*)bit_pattern_31_;
//使用std::back_inserter的目的是可以快覆盖掉这个容器pattern之前的数据
//其实这里的操作就是，将在全局变量区域的、int格式的随机采样点以cv::point格式复制到当前类对象中的成员变量中
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
特征点的旋转计算
umax.resize(HALF_PATCH_SIZE + 1);
//cvFloor返回不大于参数的最大整数值，cvCeil返回不小于参数的最小整数值，cvRound则是四舍五入
int v, //循环辅助变量
v0, //辅助变量
vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); //计算圆的最大行号，+1应该是把中间行也给考虑进去了
//NOTICE 注意这里的最大行号指的是计算的时候的最大行号，此行的和圆的角点在45°圆心角的一边上，之所以这样选择
//是因为圆周上的对称特性
//这里的二分之根2就是对应那个45°圆心角
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
//半径的平方
const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
//利用圆的方程计算每行像素的u坐标边界（max）
for (v = 0; v <= vmax; ++v)
umax[v] = cvRound(sqrt(hp2 - v * v)); //结果都是大于0的结果，表示x坐标在这一行的边界
// Make sure we are symmetric
//这里其实是使用了对称的方式计算上四分之一的圆周上的umax，目的也是为了保持严格的对称（如果按照常规的想法做，由于cvRound就会很容易出现不对称的情况，
//同时这些随机采样的特征点集也不能够满足旋转之后的采样不变性了）
for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
{
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}
}//
ORBextractor::ORBextractor 结束
//
如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器
if(sensor==System::STEREO)
mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
单目初始化的时候，会用mpIniORBextractor来作为特征点提取器
// 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器
if(sensor==System::MONOCULAR)
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
判断一个3D点远/近的阈值 mbf * 35 / fx
if(sensor==System::STEREO || sensor==System::RGBD)
{
// 判断一个3D点远/近的阈值 mbf * 35 / fx
//ThDepth其实就是表示基线长度的多少倍
mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
}
if(sensor==System::RGBD)
{
// 深度相机disparity转化为depth时的因子
mDepthMapFactor = fSettings["DepthMapFactor"];
if(fabs(mDepthMapFactor)<1e-5)
mDepthMapFactor=1;
else
mDepthMapFactor = 1.0f/mDepthMapFactor;
}
}