当前位置：首页 > news >正文

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

news 2025/2/22 2:16:31

前言

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

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

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

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

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

1.函数声明

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

2.函数定义

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

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

// mbTrackInView是决定一个地图点是否进行重投影的标志// 这个标志的确定要经过多个函数的确定，isInFrustum()只是其中的一个验证关卡。这里默认设置为否pMP->mbTrackInView = false;// 3D in absolute coordinatescv::Mat P = pMP->GetWorldPos(); // 3D in camera coordinates// 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pcconst 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 depthif(PcZ<0.0f)return false;

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

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

 // Project in image and check it is not outsideconst 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

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

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

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

 // Check viewing anglecv::Mat Pn = pMP->GetNormal();// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值const float viewCos = PO.dot(Pn)/dist;//PO和Pn点积在投影又因为Pn为单位向量所以直接得到余弦值。//夹角要在60°范围内，否则认为观测方向太偏了，重投影不可靠，返回falseif(viewCos<viewingCosLimit)return false;

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

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

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

 // 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;					//执行到这里说明这个地图点在相机的视野中并且进行重投影是可靠的，返回truereturn true;