在追踪线程的一开始就会创建一个帧
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im,const double ×tamp)
构造函数
在构造函数中,会对特征点进行提取。
ExtractORB(0,imGray);
特征点分配至网格
将图像划分为48*64的网格,然后将特征点放入每个网格中。
if(mbInitialComputations)
{
// 计算去畸变后图像的边界
ComputeImageBounds(imGray);
// 表示一个图像像素相当于多少个图像网格列(宽)
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
// 表示一个图像像素相当于多少个图像网格行(高)
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
//给类的静态成员变量复制
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
// 猜测是因为这种除法计算需要的时间略长,所以这里直接存储了这个中间计算结果
invfx = 1.0f/fx;
invfy = 1.0f/fy;
//特殊的初始化过程完成,标志复位
mbInitialComputations=false;
}
//计算 basline
mb = mbf/fx;
// 将特征点分配到图像网格中
AssignFeaturesToGrid();
将特征点分配到网格中
void Frame::AssignFeaturesToGrid()
判断路标点是否在当前帧的视野中
(1)相机坐标系下,其深度应大于0
// Step 2 关卡一:检查这个地图点在当前帧的相机坐标系下,是否有正的深度.如果是负的,表示出错,直接返回false
if(PcZ<0.0f)
return false;
(2)投影到图像后不能越界
if(u<mnMinX || u>mnMaxX)
return false;
if(v<mnMinY || v>mnMaxY)
return false;
(3)相机光心----路标点间的长度要在范围内
// Step 4 关卡三:计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内
// 得到认为的可靠距离范围:[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(dist<minDistance || dist>maxDistance)
return false;
(4)当前的观测方向和路标点的平均方向夹角过大
// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值,注意平均观测方向为单位向量
const float viewCos = PO.dot(Pn)/dist;
//如果大于给定的阈值 cos(60°)=0.5,认为这个点方向太偏了,重投影不可靠,返回false
if(viewCos<viewingCosLimit)
return false;
(5)设置该路标点的属性:估测的尺度、路标点投影到当前帧的像素坐标
寻找领域内的特征点
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
遍历路标点投影位置周围的像素框,然后遍历像素框中的特征点,计算其和投影位置间的长度,如果小于r即为候选特征点。