ORB-SLAM2源码学习：Frame.cc: Frame::isInFrustum 判断地图点是否在当前帧的视野范围内

前言

这一部分是为了选定在当前帧的视野范围内的地图点，需要满足4个条件才能入选：

条件1.将这个地图点变换到当前帧的相机坐标系下，满足深度值为正。

条件2.将地图点投影到当前帧的像素坐标系下，满足在图片的有效范围内。

条件3.计算地图点到相机中心的距离，满足在有效范围内。

条件4.计算当前相机指向地图点的向量与地图点的平均观测方向的夹角，满足小于60°（实际比较的是余弦值）。

1.函数声明

cpp 复制代码

/*
 pMP                       当前地图点
 viewingCosLimit           当前相机指向地图点向量和地图点的平均观测方向夹角余弦阈值
 return true                         地图点合格，且在视野内
 return false                        地图点不合格，抛弃
*/
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{
    ....
}

2.函数定义

1.判断其是否满足条件1

获得这个地图点的世界坐标并将其转换到当前相机坐标系下

cpp 复制代码

// mbTrackInView是决定一个地图点是否进行重投影的标志
    // 这个标志的确定要经过多个函数的确定，isInFrustum()只是其中的一个验证关卡。这里默认设置为否
    pMP->mbTrackInView = false;

    // 3D in absolute coordinates
    cv::Mat P = pMP->GetWorldPos(); 

    // 3D in camera coordinates
    // 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc
    const cv::Mat Pc = mRcw*P+mtcw; 
    const float &PcX = Pc.at<float>(0);
    const float &PcY = Pc.at<float>(1);
    const float &PcZ = Pc.at<float>(2);

    // Check positive depth
    if(PcZ<0.0f)
        return false;

2. 判断其是否满足条件2

将地图点投影到当前帧的像素坐标下

cpp 复制代码

 // 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;			

    // 判断是否在图像边界中，只要不在那么就说明无法在当前帧下进行重投影
    if(u<mnMinX || u>mnMaxX)
        return false;
    if(v<mnMinY || v>mnMaxY)
        return false;

3.判断其是否满足条件3

计算地图点到相机中心的距离

cpp 复制代码

// Check distance is in the scale invariance region of the 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.判断其是否满足条件4

计算当前相机指向地图点的向量与平均观测方向的夹角，

cpp 复制代码

 // Check viewing angle
    cv::Mat Pn = pMP->GetNormal();

	// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值
    const float viewCos = PO.dot(Pn)/dist;//PO和Pn点积在投影又因为Pn为单位向量所以直接得到余弦值。

	//夹角要在60°范围内，否则认为观测方向太偏了，重投影不可靠，返回false
    if(viewCos<viewingCosLimit)
        return false;

5.预测尺度并记录一些参数

调用PredictScale（）函数根据距离预测地图点对应的特征点所在的金字塔尺度。

ORB-SLAM2源码学习：MapPoint.cc: MapPoint::PredictScale()预测一个尺度

cpp 复制代码

 // 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;	

    // bf/z其实是视差，相减得到右图（如有）中对应点的横坐标
    pMP->mTrackProjXR = u - mbf*invz; 

	// 该地图点投影在当前图像（一般是左图）的像素纵坐标									
    pMP->mTrackProjY = v;				

    // 根据地图点到光心距离，预测的该地图点的尺度层级
    pMP->mnTrackScaleLevel = nPredictedLevel;		

    // 保存当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值
    pMP->mTrackViewCos = viewCos;					

    //执行到这里说明这个地图点在相机的视野中并且进行重投影是可靠的，返回true
    return true;

结束语

以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。