一、Frame::isInFrustum

1. 函数讲解

此函数判断地图点是否在视野中，只有同时满足Step 2、Step 3、Step 4、Step 5

• Step 1 获得这个地图点的世界坐标，经过以下层层关卡的判断，通过的地图点才认为是在视野中
• Step 2 关卡一：将这个地图点变换到当前帧的相机坐标系下，如果深度值为正才能继续下一步。
• Step 3 关卡二：将地图点投影的到当前帧像素坐标，如果在图像有效范围内才能继续下一步。
• Step 4 关卡三：计算地图点到相机中心的距离，如果在有效距离范围内才能继续下一步。
• Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于60°才能进入下一步。
• Step 6 根据地图点到光心的距离来预测一个尺度（仿照特征点金字塔层级）
• Step 7 记录计算得到的一些参数

2. 源码

bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{// mbTrackInView是决定一个地图点是否进行重投影的标志// 这个标志的确定要经过多个函数的确定，isInFrustum()只是其中的一个验证关卡。这里默认设置为否pMP->mbTrackInView = false;// 3D in absolute coordinates// 获得当前地图点的世界坐标cv::Mat P = pMP->GetWorldPos(); //  在MapPoint中定义// 3D in camera coordinates// 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pcconst cv::Mat Pc = mRcw*P+mtcw;// 读取x，y，z坐标const float &PcX = Pc.at<float>(0);const float &PcY= Pc.at<float>(1);const float &PcZ = Pc.at<float>(2);// Check positive depth// 如果深度小于0，则返回false（不投影）if(PcZ<0.0f)return false;// Project in image and check it is not outside// 将地图点投影到当前帧的像素坐标，如果在图像有效范围内才能继续下一步const float invz = 1.0f/PcZ;const float u=fx*PcX*invz+cx;const float v=fy*PcY*invz+cy;// 判读特征点是否在边界类，不在就返回false（不投影）if(u<mnMinX || u>mnMaxX)return false;if(v<mnMinY || v>mnMaxY)return false;// Check distance is in the scale invariance region of the MapPoint// 计算地图点到相机中心的距离，如果在有效距离范围内才能继续下一步// 获取认可的地图点到相机中心的距离const float maxDistance = pMP->GetMaxDistanceInvariance();const float minDistance = pMP->GetMinDistanceInvariance();// 得到当前地图点距离当前帧相机光心的距离const cv::Mat PO = P-mOw;// 取模就得到了距离const float dist = cv::norm(PO);// 如果不在范围内，就返回falseif(dist<minDistance || dist>maxDistance)return false;// Check viewing angle// 计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于60°才能进入下一步。cv::Mat Pn = pMP->GetNormal(); // 在MapPoint中定义// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量const float viewCos = PO.dot(Pn)/dist;// 如果夹角超出范围，就返回falseif(viewCos<viewingCosLimit)return false;// Predict scale in the image// 根据地图点到光心的距离来预测一个尺度（仿照特征点金字塔层级）const int nPredictedLevel = pMP->PredictScale(dist,this);// Data used by the tracking// 通过置位标记 MapPoint::mbTrackInView 来表示这个地图点要被投影 pMP->mbTrackInView = true;                  pMP->mTrackProjX = u;                       // 该地图点投影在当前图像（一般是左图）的像素横坐标pMP->mTrackProjXR = u - mbf*invz;           // bf/z其实是视差，相减得到右图（如有）中对应点的横坐标pMP->mTrackProjY = v;                       // 该地图点投影在当前图像（一般是左图）的像素纵坐标pMP->mnTrackScaleLevel= nPredictedLevel;    // 根据地图点到光心距离，预测的该地图点的尺度层级pMP->mTrackViewCos = viewCos;               // 保存当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值// 前面的条件都符合，就返回truereturn true;
}

二、Frame::GetFeaturesInArea

1. 函数讲解

本函数的作用是，找出以（x，y）为中心，以r为半径的圆内的所有特征点，方法为，先找到包含这个圆的最小网格边界（四条红色线形成的方形网格），因为前面我们将特征存入一个个网格中了，这样更方便查找。然后找出这个最小边界网格中的每一个小网格内的特征点，并记录其坐标（中间有涉及到坐标变换，是为了方便计算距离），最后计算这个坐标到（x，y）的距离，如果大于r，则抛弃（在圆外）

2. 函数源码

// 找到符合条件的特征点（以x,y为中心,半径为r的圆形内且金字塔层级在[minLevel, maxLevel]）
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const
{// 开辟大小为N的特征点容器vector<size_t> vIndices;vIndices.reserve(N);// 计算半径为r圆左右上下边界所在的网格列和行的idconst int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));if(nMinCellX>=FRAME_GRID_COLS)return vIndices;// 计算圆所在的右边界网格列索引const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));if(nMaxCellX<0)return vIndices;// 计算圆所在的上边界网格列索引const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));if(nMinCellY>=FRAME_GRID_ROWS)return vIndices;// 计算圆所在的下边界网格列索引const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));if(nMaxCellY<0)return vIndices;// 这个判断条件有问题，改为const bool bCheckLevels = (minLevel>=0) || (maxLevel>=0);const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);// 遍历这个区域，找出这个区域内的所有特征点for(int ix = nMinCellX; ix<=nMaxCellX; ix++){for(int iy = nMinCellY; iy<=nMaxCellY; iy++){const vector<size_t> vCell = mGrid[ix][iy];if(vCell.empty())continue;// 读取每一个特征点，判断其特征点层级是否在给定范围（决定于传进来的参数minLevel，maxLeve）for(size_t j=0, jend=vCell.size(); j<jend; j++){const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];if(bCheckLevels){if(kpUn.octave<minLevel)continue;if(maxLevel>=0)if(kpUn.octave>maxLevel)continue;}// 判断特征点是否在圆内，如是在圆内就记录const float distx = kpUn.pt.x-x;const float disty = kpUn.pt.y-y;if(fabs(distx)<r && fabs(disty)<r)vIndices.push_back(vCell[j]);}}}return vIndices;
}// 确定特征点在网格中的位置，返回值为bool类型
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{// 计算特征点在哪个网格内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// 超出边界返回falseif(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)return false;// 可以正常分配返回truereturn true;
}

三、Frame::ComputeBoW

1. 函数讲解

本函数的作用是将描述子转化成词袋BoW的模式，这样的好处是，在后续的特征点匹配中，先找出对应的词袋，只需要在这个词袋范围内来逐个匹配，这样大大的减少了计算量和计算时间

2. 函数源码

void Frame::ComputeBoW()
{   // 判断是否以前已经计算过了，计算过了就跳过if(mBowVec.empty()){// 将描述子mDescriptors转换为DBOW要求的输入格式vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);// 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVecmpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);}
}

四、Frame::UnprojectStereo

1. 函数讲解

本函数是将符合条件的特征点反投影到三维世界坐标中，在地图点创建中，这个步骤尤为重要，这个段代码的后两行是冗余的，在主线程中调用此函数前，都筛选过深度

2. 函数源码

// 将符合条件的特征点反投影到三维世界坐标中
cv::Mat Frame::UnprojectStereo(const int &i)
{// 记录特征点深度，const float z = mvDepth[i];// 深度大于0则（明显是多人编程，在计算深度时已经判断过了，这里是冗余的），计算其三维坐标，并转换到世界坐标if(z>0){// 获取去畸变后的特征点坐标const float u = mvKeysUn[i].pt.x;const float v = mvKeysUn[i].pt.y;// 计算当前相机坐标系下的坐标const float x = (u-cx)*z*invfx;const float y = (v-cy)*z*invfy;// 将其转换到三维坐标系下cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);// 将其转换为世界坐标return mRwc*x3Dc+mOw;}// 如果深度不合格，就返回空矩阵（冗余代码）elsereturn cv::Mat();
}

五、总结

这四个函数不在Frame.cc中调用，但是作用很重要，帧在三个线程中被普遍应用，如果帧的内容不熟悉，学三个主线程时会有很多的疑问。这两个函数因为比较简单，并且本人对代码做了很详细的标注，所以没有过多的函数讲解，若大家有疑问欢迎讨论。