提供了编程的基础技术教程

zl程序教程

您现在的位置是：首页 >  其它

当前栏目

ORB-SLAM2 --- Frame::isInFrustum函数

函数 --- Frame ORB SLAM2
2023-09-14 09:01:37 时间

目录

1.函数作用

2.函数流程 

3.code 

4.函数解析 

1.函数作用

        判断地图点是否在视野中。

2.函数流程 

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

3.code 

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<float>(0);
    const float &PcY = Pc.at<float>(1);
    const float &PcZ = Pc.at<float>(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(u<mnMinX || u>mnMaxX)
        return false;
    if(v<mnMinY || v>mnMaxY)
        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(dist<minDistance || dist>maxDistance)
        return false;

    // Check viewing angle
    // Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于60°才能进入下一步。
    cv::Mat Pn = pMP->GetNormal();

	// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量
    const float viewCos = PO.dot(Pn)/dist;

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

    // Predict scale in the image
    // Step 6 根据地图点到光心的距离来预测一个尺度（仿照特征点金字塔层级）
    const int nPredictedLevel = pMP->PredictScale(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;
}

4.函数解析 

        首先获得该地图点的世界坐标P，然后用先前的三种方式之一得到的“粗糙的”变换方式mRcw及mtcw将该地图点从世界坐标系转到相机坐标系中。得到相机坐标系下的坐标PcX，PcY，PcZ。

        判断一：看看这个在相机坐标系下的点深度是否为负（PcZ<0）

        再将这个地图点投影到当前帧的像素坐标系中，看起是否在有效范围：

Z\begin{bmatrix} u\\v \\1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y &c_y \\ 0 & 0 &1 \end{bmatrix}\begin{bmatrix} X\\Y \\1 \end{bmatrix}

        判断二：判断该点投影到像素坐标系下是否在有效范围

        判断三：计算地图点到相机中心的距离，看是否在有效范围

        判断四：计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于60°才能进入下一步

5.补充事宜

        有关计算有效距离与计算当前相机指向地图点向量和地图点的平均观测方向夹角是通过MapPoint::UpdateNormalAndDepth函数获得的。

        该函数的工作流程如下：

         这里的mObservations是一个map<KeyFrame*,size_t>型的变量，存储了能观测到该地图点的关键帧和该地图点在该关键帧中的索引号。mpRefKF是该帧的参考关键帧（即该帧mCurrenrFrame一级共视关键帧中共视程度最大的一帧）。

        我们遍历mObservations中的每项，取出能观测到该地图点的帧的相机在世界坐标系的坐标，在计算出相机中心指向地图点的向量并归一化，如上图。

        最后我们得到了至关重要的三个量：

        地图点平均的观测方向mNormalVector ：描述了地图点被平均观测到的点，即无数个个相机观测到该地图点的角度的均值。

        观测到该点的距离上限mfMaxDistance ：参考关键帧相机光心到地图点的距离 * 尺度因子

        观测到该点的距离下限mfMinDistance： 参考关键帧相机光心到地图点的距离 * 尺度因子

        那么我们再理解上面的函数是不是就知道这个变量是在哪赋值的。

    const float maxDistance = pMP->GetMaxDistanceInvariance();

float MapPoint::GetMaxDistanceInvariance()
{
    unique_lock<mutex> lock(mMutexPos);
    return 1.2f*mfMaxDistance;
}

// 下图中横线的大小表示不同图层图像上的一个像素表示的真实物理空间中的大小
//              ____
// Nearer      /____\     level:n-1 --> dmin
//            /______\                       d/dmin = 1.2^(n-1-m)
//           /________\   level:m   --> d
//          /__________\                     dmax/d = 1.2^m
// Farther /____________\ level:0   --> dmax
//
//           log(dmax/d)
// m = ceil(------------)
//            log(1.2)
这个函数的作用：
        在进行投影匹配的时候会给定特征点的搜索范围，考虑到处于不同尺度（也就是距离相机远近，位于图像金字塔中不同图层）的特征点受到相机旋转的影响不同，因此会希望距离相机近的点的搜索范围更大一点，距离相机更远的点的搜索范围更小一点，所以要在这里，根据点到关键帧/帧的距离来估计它在当前的关键帧/帧中会大概处于哪个尺度。

/**
 * @brief 预测地图点对应特征点所在的图像金字塔尺度层数
 * 
 * @param[in] currentDist   相机光心距离地图点距离
 * @param[in] pKF           关键帧
 * @return int              预测的金字塔尺度
 */
int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF)
{
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        // mfMaxDistance = ref_dist*levelScaleFactor 为参考帧考虑上尺度后的距离
        // ratio = mfMaxDistance/currentDist = ref_dist/cur_dist
        ratio = mfMaxDistance/currentDist;
    }

    // 取对数
    int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pKF->mnScaleLevels)
        nScale = pKF->mnScaleLevels-1;

    return nScale;
}


/**
 * @brief 根据地图点到光心的距离来预测一个类似特征金字塔的尺度
 * 
 * @param[in] currentDist       地图点到光心的距离
 * @param[in] pF                当前帧
 * @return int                  尺度
 */
int MapPoint::PredictScale(const float &currentDist, Frame* pF)
{
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        ratio = mfMaxDistance/currentDist;
    }

    int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pF->mnScaleLevels)
        nScale = pF->mnScaleLevels-1;

    return nScale;
}
void MapPoint::UpdateNormalAndDepth()
{
    // Step 1 获得观测到该地图点的所有关键帧、坐标等信息
    map<KeyFrame*,size_t> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;

        observations=mObservations; // 获得观测到该地图点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧（第一次创建时的关键帧）
        Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置
    }

    if(observations.empty())
        return;

    // Step 2 计算该地图点的平均观测方向
    // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向
    // 初始值为0向量，累加为归一化向量，最后除以总数n
    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        // 获得地图点和观测到它关键帧的向量并归一化
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali);                       
        n++;
    } 

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC);                                        // 该点到参考关键帧相机的距离
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        // 观测到该地图点的当前帧的特征点在金字塔的第几层
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 当前金字塔层对应的尺度因子，scale^n，scale=1.2，n为层数
    const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔总层数，默认为8

    {
        unique_lock<mutex> lock3(mMutexPos);
        // 使用方法见PredictScale函数前的注释
        mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
        mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
    }
}

相关文章