个人笔记，如有错误，敬请指正
本篇笔记意在分析 ORBSLAM2 从开始运行，到完成FAST关键点提取，所经过的整个路径。
笔记大纲
程序入口
rgbd_tum.cc main 函数
(rgbd_tum.cc) main() 中会创建 (System.cc) [System] 类对象 SLAM，利用其构造函数完成 SLAM 系统的初始化，传入字典路径、yaml 参数文件路径以及相机类型等信息，相关代码如下所示：
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
(rgbd_tum.cc) main() 接下来会进入主循环中，开始读取每一帧彩色图与深度图，将其保存到 imRGB 以及 imD 两个变量中。
(rgbd_tum.cc) main() 接着调用 (System.cc)[System::SLAM] TrackRGBD() ，将当前帧的彩色图和深度图传入，进行一些参数的判断与设置，如下所示：
// Pass the image to the SLAM system
SLAM.TrackRGBD(imRGB,imD,tframe);
System.cc TrackRGBD 函数
(System.cc)[System::SLAM] TrackRGBD() 将调用 (Tracking.cc)[Tracking] 类的对象 (System.cc)[System::SLAM] mpTracker 的 (Tracking.cc)[Track::mpTracker] GrabImageRGBD() ，将彩色图和深度图传入，如下所示：
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
Tracking.cc GrabImageRGBD 函数
(Tracking.cc)[Track::mpTracker] GrabImageRGBD() 将RGB三通道的彩色图转换为单通道的灰度图，并创建新的 Frame 对象，保存到(Tracking.cc)[Track::mpTracker] mCurrentFrame 成员变量中，如下所示：
mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
Track();
return mCurrentFrame.mTcw.clone();
在创建 Frame 对象的同时，也会传入灰度图像、深度图像、时间戳、ORB特征提取器、ORB词典等参数，由此可见，特征点的提取，以及描述子的计算，主要在 Frame 的构造函数中完成。
Frame 构造函数
参数分析
const cv::Mat &imGray // 灰度图
const cv::Mat &imDepth // 深度图
const double &timeStamp // 时间戳
ORBextractor* extractor // ORB特征提取器
ORBVocabulary* voc // 字典
cv::Mat &K // 相机内参矩阵
cv::Mat &distCoef // 图像校正参数，用于 UndistortKeyPoints 函数中
const float &bf // 与焦距 fx 相乘的基线值，用于ComputeStereoFromRGBD 函数中
const float &thDepth // 深度阈值
过程分析
// Frame ID
mnId=nNextId++;
// Scale Level Info
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// ORB extraction
ExtractORB(0,imGray);
// 特征点数目，自此不再改变
N = mvKeys.size();
// 校正特征点的像素坐标，获取无畸变的特征点的像素坐标
// 原来的特征点保存在 mvKeys
// 校正的特征点保存在 mvKeysUn
// 最终使用的仍然是校正后的特征点
UndistortKeyPoints();
// 与后续 Track 函数中特征点的处理有关
// 只影响 mvuRight 以及 mvDepth 两个变量
ComputeStereoFromRGBD(imDepth);
// 初始化地图点向量，大小即特征点数目
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
// 特征点是否为外点的标记向量，大小即特征点数目
mvbOutlier = vector<bool>(N,false);
mb = mbf/fx;
// 划分特征点到对应的栅格中
// 从而加速对特征点的搜索
AssignFeaturesToGrid();
(Frame.cc)[Frame::mCurrentFrame] ExtractORB() 负责特征点的提取，其代码如下：
void Frame::ExtractORB(int flag, const cv::Mat &im)
{
if(flag==0)
(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
else
(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}
由上述代码可见，程序最终转入到 mpORBextractorLeft 的运算符重载函数 (ORBextractor.cc)[ORBextractor::mpORBextractorLeft] ORBextractor::operator() 中。
ORBextractor 运算符重载函数
参数分析
InputArray _image // 灰度图
InputArray _mask // 掩码
vector<KeyPoint>& _keypoints // 关键点，此处为 Frame 类的成员变量 mvKeys
OutputArray _descriptors) // 描述子
过程分析
// InputArray 类型转换为 Mat 类型
Mat image = _image.getMat();
// 必须为灰度图
assert(image.type() == CV_8UC1 );
// 建立图像金字塔
ComputePyramid(image);
// 提取每层金字塔的关键点
// 保存在 allKeypoints
vector < vector<KeyPoint> > allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);
//ComputeKeyPointsOld(allKeypoints);
// 存储关键点的描述子
Mat descriptors;
// 计算关键点的总数目
// 保存在 nkeypoints
int nkeypoints = 0;
for (int level = 0; level < nlevels; ++level)
nkeypoints += (int)allKeypoints[level].size();
if( nkeypoints == 0 )
_descriptors.release();
// descriptors 实质是 nkeyypoints × 32 的矩阵
// uchar 类型的元素有 8 bits
// 相当于每个关键点，拥有 32 × 8 = 256 bits 的描述子
else
{
_descriptors.create(nkeypoints, 32, CV_8U);
descriptors = _descriptors.getMat();
}
// 自此描述子暂时存储在局部变量 descriptors
_keypoints.clear();
_keypoints.reserve(nkeypoints);
// 遍历高斯金字塔
int offset = 0; // 从描述子矩阵的第 0 行开始，亦即第一个关键点
for (int level = 0; level < nlevels; ++level)
{
// 获取此层的所有关键点，存储于 keypoints
vector<KeyPoint>& keypoints = allKeypoints[level];
// 计算其数量，存储于 nkeypointsLevel
int nkeypointsLevel = (int)keypoints.size();
if(nkeypointsLevel==0)
continue;
// workingMat 存储此层的金字塔图像
Mat workingMat = mvImagePyramid[level].clone();
// 对其进行高斯模糊
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
// rowRange 函数获取指定行范围的子矩阵指针，注意这里是指针
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
// 计算此层金字塔的描述子
computeDescriptors(workingMat, keypoints, desc, pattern);
// 更新偏移量
offset += nkeypointsLevel;
// 恢复关键点在原图中的坐标
if (level != 0)
{
float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end();
keypoint != keypointEnd; ++keypoint)
keypoint->pt *= scale;
}
// 在 _keypoints 的末尾追加此层的关键点 keypoints
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
}
由上述代码可见，关键点的提取在于ComputeKeyPointsOctTree函数中，并将提取得到的特征点存储于allKeypoints中。
ComputeKeyPointsOctTree 函数
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{
allKeypoints.resize(nlevels);
// 分割窗口的边长
const float W = 30;
// 对每层金字塔的图像进行遍历，逐个提取FAST关键点
for (int level = 0; level < nlevels; ++level)
{
// 确定提取FAST关键点的范围，太边缘的关键点则放弃提取
const int minBorderX = EDGE_THRESHOLD-3;
const int minBorderY = minBorderX;
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
// 存储待筛选的FAST关键点
vector<cv::KeyPoint> vToDistributeKeys;
vToDistributeKeys.reserve(nfeatures*10);
// 计算边界的宽度以及高度
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
// 分割的行数以及列数
const int nCols = width/W;
const int nRows = height/W;
// 每个分割窗口的宽度以及高度
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);
// 利用两个 for 循环遍历所有的分割窗口
// 对每一行进行遍历
for(int i=0; i<nRows; i++)
{
// 确定行的上边界坐标 maxY
// 确定行下边界坐标 iniY
const float iniY =minBorderY+i*hCell;
float maxY = iniY+hCell+6;
if(iniY>=maxBorderY-3)
continue;
if(maxY>maxBorderY)
maxY = maxBorderY;
// 对每一列进行遍历
for(int j=0; j<nCols; j++)
{
// 确定列的左边界坐标 iniX
// 确定列的右边界坐标 maxX
const float iniX =minBorderX+j*wCell;
float maxX = iniX+wCell+6;
if(iniX>=maxBorderX-6)
continue;
if(maxX>maxBorderX)
maxX = maxBorderX;
// vKeysCell 存储当前分割窗口内的所有FAST关键点
vector<cv::KeyPoint> vKeysCell;
// rowRange 返回对应行范围的子矩阵
// colRange 返回对应列范围的子矩阵
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,iniThFAST,true);
// 若提取不到关键点，就使用降低后的阈值 minThFAST
if(vKeysCell.empty())
{
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,minThFAST,true);
}
// 若提取到关键点，则将其加入 vToDistributeKeys 中
if(!vKeysCell.empty())
{
for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
{
// 由于 vKeysCell 中关键点的坐标是分割窗口子矩阵中的坐标
// 因此在此处做一个坐标恢复
(*vit).pt.x+=j*wCell;
(*vit).pt.y+=i*hCell;
vToDistributeKeys.push_back(*vit);
}
}
}
}
// 引用类型向量 keypoints 指向 allKeypoints 中存储当前层金字塔关键点的向量
// keypoints 实际存储经 DistributeOctTree 筛选后的关键点
vector<KeyPoint> & keypoints = allKeypoints[level];
keypoints.reserve(nfeatures);
// 利用 DistributeOctTree 函数实现对关键点的筛选
keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
// 由于FAST关键点是在经过缩放后的金字塔图像中存储的，因此需要对其相关信息进行尺度上的恢复
const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
// Add border to coordinates and scale information
const int nkps = keypoints.size();
for(int i=0; i<nkps ; i++)
{
keypoints[i].pt.x+=minBorderX;
keypoints[i].pt.y+=minBorderY;
keypoints[i].octave=level;
keypoints[i].size = scaledPatchSize;
}
}
// 计算关键点的方向
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
至此，ORBSLAM2 中对FAST关键点的提取过程就算彻底完成了。
附录：ORBextractor.h
成员变量
public:
std::vector<cv::Mat> mvImagePyramid; // 每层金字塔的图像
protected:
// 以下五个参数从 TUM1.yaml 文件中读取
int nfeatures; // 特征点提取的最大数量，读取为 1000
double scaleFactor; // 每层金字塔之间的缩放比例，读取为 1.2，实际是其倒数，因此为下采样
int nlevels; // 金字塔的层数，读取为 8
int iniThFAST; // FAST 角点提取的初始阈值，读取为 20
int minThFAST; // FAST 角点提取的二次阈值，亦是更小的阈值，防止某层金字塔提取不到角点，读取为 7
std::vector<int> mnFeaturesPerLevel; // 每层金字塔的提取到的特征点数量
std::vector<int> umax; // Patch 圆u轴方向最大坐标
std::vector<float> mvScaleFactor; // 每层金字塔相对于原始图像的缩放比例，此处为下采样
std::vector<float> mvInvScaleFactor; // 缩放比例的倒数
std::vector<float> mvLevelSigma2; // 缩放比例的平方
std::vector<float> mvInvLevelSigma2; // 缩放比例的平方的倒数
成员函数
protected:
// 建立图像金字塔，按比例缩小图像，保存到 mvImagePyramid
void ComputePyramid(cv::Mat image);
// 利用四叉树提取每层金字塔的关键点，以使其分布均匀
void ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
// 利用四叉树筛选每层金字塔的关键点，以使其分布均已，且数量在预计的范围内
std::vector<cv::KeyPoint> DistributeOctTree(const std::vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level);
// 旧的特征点提取函数，已废弃
void ComputeKeyPointsOld(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
附录：变量命名规范
| 缩写 | 全写 | 含义 |
|---|---|---|
| m | member | 类的成员变量 |
| t | thread | 线程类型变量 |
| l | list | List 类型变量 |
| n | integer | 整型变量 |
| p | pointer | 指针类型变量 |
| b | bool | 布尔类型变量 |
| v | vector | 向量类型变量 |
| s | set | 集合类型变量 |
扫一扫
3561
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
