void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
// 如果畸变参数不为0，用OpenCV函数进行畸变矫正
if(mDistCoef.at(0)!=0.0)
{
// 保存矫正前的图像四个边界点坐标： (0,0) (cols,0) (0,rows) (cols,rows)
cv::Mat mat(4,2,CV_32F);
mat.at(0,0)=0.0; //左上
mat.at(0,1)=0.0;
mat.at(1,0)=imLeft.cols; //右上
mat.at(1,1)=0.0;
mat.at(2,0)=0.0; //左下
mat.at(2,1)=imLeft.rows;
mat.at(3,0)=imLeft.cols; //右下
mat.at(3,1)=imLeft.rows;
// Undistort corners
// 和前面校正特征点一样的操作，将这几个边界点作为输入进行校正
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1);
//校正后的四个边界点已经不能够围成一个严格的矩形，因此在这个四边形的外侧加边框作为坐标的边界
mnMinX = min(mat.at(0,0),mat.at(2,0));//左上和左下横坐标最小的
mnMaxX = max(mat.at(1,0),mat.at(3,0));//右上和右下横坐标最大的
mnMinY = min(mat.at(0,1),mat.at(1,1));//左上和右上纵坐标最小的
mnMaxY = max(mat.at(2,1),mat.at(3,1));//左下和右下纵坐标最小的
}
else
{
// 如果畸变参数为0，就直接获得图像边界
mnMinX = 0.0f;
mnMaxX = imLeft.cols;
mnMinY = 0.0f;
mnMaxY = imLeft.rows;
}
}
该方法的流程为：
1.如果畸变参数不为0,保存图像的左上，右上，左下，右下顶点
2. 通过opencv的去畸变函数 undistortPoints 进行校正，更新图像边界 mnMinX mnMaxX
mnMinY mnMaxY
3.如果畸变参数为0,则直接用输入的图像的边界当边界
二、将ORB特征点分配到图像网格中 void Frame::AssignFeaturesToGrid()void Frame::AssignFeaturesToGrid()
{
// Step 1 给存储特征点的网格数组 Frame::mGrid 预分配空间
// ? 这里0.5 是为什么？节省空间？
// FRAME_GRID_COLS = 64，FRAME_GRID_ROWS=48
int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
//开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间
for(unsigned int i=0; i
该方法的流程为：
1.给每个格子存储的特征点队列 mGrid 预先分配存储空间。
2. 遍历每个去畸变后的特征点，如果特征点的坐标在网格范围内则将特征点的索引写到对应的mGrid 中。
三、提取图像的ORB特征 void Frame::ExtractORB(int flag, const cv::Mat &im)
void Frame::ExtractORB(int flag, const cv::Mat &im)
{
// 判断是左图还是右图
if(flag==0)
// 左图的话就套使用左图指定的特征点提取器，并将提取结果保存到对应的变量中
// 这里使用了仿函数来完成，重载了括号运算符 ORBextractor::operator()
(*mpORBextractorLeft)(im, //待提取特征点的图像
cv::Mat(), //掩摸图像, 实际没有用到
mvKeys, //输出变量，用于保存提取后的特征点
mDescriptors); //输出变量，用于保存特征点的描述子
else
// 右图的话就需要使用右图指定的特征点提取器，并将提取结果保存到对应的变量中
(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}
该方法的流程为：
判断是左图还是右图，通过ORBextractor.cc 内重载的()运算符获得提取到的特征点和描述子
ORBextractor.cc 到时候单独讲。
四、设置相机姿态 void Frame::SetPose(cv::Mat Tcw)
void Frame::SetPose(cv::Mat Tcw)
{
mTcw = Tcw.clone();
UpdatePoseMatrices();
}
直接讲下面
五、 根据Tcw计算mRcw、mtcw和mRwc、mOw void Frame::UpdatePoseMatrices()
void Frame::UpdatePoseMatrices()
{
// mOw： 当前相机光心在世界坐标系下坐标
// mTcw： 世界坐标系到相机坐标系的变换矩阵
// mRcw： 世界坐标系到相机坐标系的旋转矩阵
// mtcw： 世界坐标系到相机坐标系的平移向量
// mRwc： 相机坐标系到世界坐标系的旋转矩阵
//从变换矩阵中提取出旋转矩阵
//注意，rowRange这个只取到范围的左边界，而不取右边界
mRcw = mTcw.rowRange(0,3).colRange(0,3);
// mRcw求逆即可
mRwc = mRcw.t();
// 从变换矩阵中提取出旋转矩阵
mtcw = mTcw.rowRange(0,3).col(3);
// mTcw 求逆后是当前相机坐标系变换到世界坐标系下，对应的光心变换到世界坐标系下就是 mTcw的逆 中对应的平移向量
mOw = -mRcw.t()*mtcw;
}
这里也是直接看注释吧，注释很清楚了。
六、判断地图点是否在视野中 bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{
// mbTrackInView是决定一个地图点是否进行重投影的标志
// 这个标志的确定要经过多个函数的确定，isInFrustum()只是其中的一个验证关卡。这里默认设置为否
pMP->mbTrackInView = false;
// 3D in absolute coordinates
// Step 1 获得这个地图点的世界坐标
cv::Mat P = pMP->GetWorldPos();
// 3D in camera coordinates
// 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc
const cv::Mat Pc = mRcw*P+mtcw;
const float &PcX = Pc.at(0);
const float &PcY = Pc.at(1);
const float &PcZ = Pc.at(2);
// Check positive depth
// Step 2 关卡一：将这个地图点变换到当前帧的相机坐标系下，如果深度值为正才能继续下一步。
if(PcZ<0.0f)
return false;
// Project in image and check it is not outside
// Step 3 关卡二：将地图点投影到当前帧的像素坐标，如果在图像有效范围内才能继续下一步。
const float invz = 1.0f/PcZ;
const float u=fx*PcX*invz+cx;
const float v=fy*PcY*invz+cy;
// 判断是否在图像边界中，只要不在那么就说明无法在当前帧下进行重投影
if(umnMaxX)
return false;
if(vmnMaxY)
return false;
// Check distance is in the scale invariance region of the MapPoint
// Step 4 关卡三：计算地图点到相机中心的距离，如果在有效距离范围内才能继续下一步。
// 得到认为的可靠距离范围:[0.8f*mfMinDistance, 1.2f*mfMaxDistance]
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
// 得到当前地图点距离当前帧相机光心的距离,注意P，mOw都是在同一坐标系下才可以
// mOw：当前相机光心在世界坐标系下坐标
const cv::Mat PO = P-mOw;
//取模就得到了距离
const float dist = cv::norm(PO);
//如果不在有效范围内，认为投影不可靠
if(distmaxDistance)
return false;
// Check viewing angle
// Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于60°才能进入下一步。
cv::Mat Pn = pMP->GetNormal();
// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量
const float viewCos = PO.dot(Pn)/dist;
//夹角要在60°范围内，否则认为观测方向太偏了，重投影不可靠，返回false
if(viewCosPredictScale(dist, //这个点到光心的距离
this); //给出这个帧
// Step 7 记录计算得到的一些参数
// Data used by the tracking
// 通过置位标记 MapPoint::mbTrackInView 来表示这个地图点要被投影
pMP->mbTrackInView = true;
// 该地图点投影在当前图像（一般是左图）的像素横坐标
pMP->mTrackProjX = u;
// bf/z其实是视差，相减得到右图（如有）中对应点的横坐标
pMP->mTrackProjXR = u - mbf*invz;
// 该地图点投影在当前图像（一般是左图）的像素纵坐标
pMP->mTrackProjY = v;
// 根据地图点到光心距离，预测的该地图点的尺度层级
pMP->mnTrackScaleLevel = nPredictedLevel;
// 保存当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值
pMP->mTrackViewCos = viewCos;
//执行到这里说明这个地图点在相机的视野中并且进行重投影是可靠的，返回true
return true;
}
这里也是看注释把写的蛮清楚的。
七、找到以x,y为中心，半径为r的圆形内，且金字塔层级在[minLevel, maxLevel]的特征点
vector Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
vector Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
{
// 存储搜索结果的vector
vector vIndices;
vIndices.reserve(N);
// Step 1 计算半径为r圆左右上下边界所在的网格列和行的id
// 查找半径为r的圆左侧边界所在网格列坐标。这个地方有点绕，慢慢理解下：
// (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素（肯定大于1）
// mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数，表示每个像素可以均分几个网格列（肯定小于1）
// (x-mnMinX-r)，可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数
// 两者相乘，就是求出那个半径为r的圆的左侧边界在哪个网格列中
// 保证nMinCellX 结果大于等于0
const int nMinCellX = max(0,(int)floor( (x-mnMinX-r)*mfGridElementWidthInv)); // nMinCellX 为圆的左边界到mnMinX占几个网格列
// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vector
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
// 计算圆所在的右边界网格列索引
// nMaxCellX 为圆右边界到mnMinX占几个网格列
const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
// 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vector
if(nMaxCellX<0)
return vIndices;
//后面的操作也都是类似的，计算出这个圆上下边界所在的网格行的id
// nMinCellY 为圆的上边界到mnMinY 所占网格数
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;
// nMinCellY 为圆的下边界到mnMinY 所占网格数
const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
// 检查需要搜索的图像金字塔层数范围是否符合要求
//? 疑似bug。(minLevel>0) 后面条件 (maxLevel>=0)肯定成立
//? 改为 const bool bCheckLevels = (minLevel>=0) || (maxLevel>=0);
const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);
// Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
// 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
const vector vCell = mGrid[ix][iy];
// 如果这个网格中没有特征点，那么跳过这个网格继续下一个
if(vCell.empty())
continue;
// 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
for(size_t j=0, jend=vCell.size(); j=0) //? 为何特意又强调？感觉多此一举
if(kpUn.octave>maxLevel)
continue;
}
// 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
// 如果x方向和y方向的距离都在指定的半径之内，存储其index为候选特征点 就是找圆内的特征点，找圆内是为了旋转不变性
if(fabs(distx)
该方法的流程为：
1.初始化一个队列 vIndices 给它特征点总个数的长度 ，用于存储搜索结果。
2. 先回顾一下
mnMinX 是去畸变后左侧最小横坐标
mnMaxX是去畸变后右侧最大横坐标
mnMinY是去畸变后上边界最小纵坐标
mnMaxY是去畸变后下边界最大纵坐标
mfGridElementWidthInv 表示一个像素等于多少个图像网格列
mfGridElementHeightInv 表示一个像素等于多少个图像网格行
先计算半径为r的圆的左边到左边界一共距离多少个像素，再乘 mfGridElementWidthInv 可以获得半径为r的圆的左边在哪个网格中。
同理，计算半径为r 的圆的右边、上边、下边在哪个网格中。这样就能得到半径为r的圆落在哪些网格中。
3.遍历待匹配网格（这里用网格的原因是加快了遍历的速度，比一个像素一个像素的匹配快），如果网格内没有特征点则退出本次循环，网格内有特征点则判断特征点是否在指定的金字塔层级内，不是的话则退出本次循环。
4.获取满足上述条件的特征点的横坐标、纵坐标，判断特征点是否在以输入x,y为中心半径为r 的圆内，在的话压入第一步的队列vIndices。
八、计算特征点所在网格的坐标 bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
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);
//Keypoint's coordinates are undistorted, which could cause to go out of the image
// 因为特征点进行了去畸变，而且前面计算是round取整，所以有可能得到的点落在图像网格坐标外面
// 如果网格坐标posX，posY超出了[0,FRAME_GRID_COLS] 和[0,FRAME_GRID_ROWS]，表示该特征点没有对应网格坐标，返回false
if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
return false;
// 计算成功返回true
return true;
}
该方法的流程为：
1. 计算特征点所在网格的纵坐标、横坐标，计算方式看七算法一样的。
2.判断网格的横坐标、纵坐标是否在阈值内，纵坐标{0, 64},横坐标{0，48}，如果在则返回true,
不在则返回false。
九、计算当前帧特征点对应的词袋Bow void Frame::ComputeBoW()
void Frame::ComputeBoW()
{
// 判断是否以前已经计算过了，计算过了就跳过
if(mBowVec.empty())
{
// 将描述子mDescriptors转换为DBOW要求的输入格式
vector vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
// 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec
mpORBvocabulary->transform(vCurrentDesc, //当前的描述子vector
mBowVec, //输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
mFeatVec, //输出，记录node id及其对应的图像 feature对应的索引
4); //4表示从叶节点向前数的层数
}
}
该算法主要 在 TemplatedVocabulary.h内 ，到时候单独将这里，只要直到
mBowVec 主要记录word 在叶子中的ID和权重 是个 map 类型的数据
mFeatVec 主要记录 节点ID和该节点ID下特征点在图像中的索引 也是个map类型的数据
