ORBSLAM2--Frame类

bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)

函数简介

该函数是运用于局部地图的跟踪里面，在进行局部地图点向当前帧进行投影前，都会对该地图点进行判断是否是处于当前帧的视野范围之内。通过像素范围、远近（深度）情况、角度情况进行判断，如果前面判断成立还会预测出对应的特征点可能出现在第几层金字塔中。

函数原型

 /*@brief 判断一个点是否在视野内
 *
 * 计算了重投影坐标，观测方向夹角，预测在当前帧的尺度
 * @param  pMP             MapPoint
 * @param  viewingCosLimit 视角和平均视角的方向阈值
 * @return                 true if is in view
 * @see SearchLocalPoints()
 */
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)

知识点

像素范围的判断

/ V-D 1) 将MapPoint投影到当前帧, 并判断是否在图像内
    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;

远近距离判断

根据前面金字塔的计算，可以推测特征点允许被成功提取的距离范围
$$d_{max}=d_m\ast 1.2^m$$
$$d_{min}=d_m\ast 1.2^{m-1}$$
其中m是该地图点在它的观测关键帧中对应特征点所在的金字塔层，这样每一个MapPoint都有一个观测到该点的帧的距离上下限，超过这范围的帧是认为是观测不到这个点的。相关介绍请看

代码片段

 // V-D 3) 计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内
    const float maxDistance = pMP->GetMaxDistanceInvariance();
    const float minDistance = pMP->GetMinDistanceInvariance();
    // 世界坐标系下，相机到3D点P的向量, 向量方向由相机指向3D点P
    const cv::Mat PO = P-mOw;
    const float dist = cv::norm(PO);

    if(dist<minDistance || dist>maxDistance)
        return false;

角度情况判断

相机会存在一个视角，每一个MapPoint都会计算出它的平均观测视角，用来当做当前地图点的观测角度向量，那可以比较当前帧观测该点的方向向量和平均观测向量的余弦值来判断是否在允许的观测视角范围内

 // V-D 2) 计算当前视角和平均视角夹角的余弦值, 若小于cos(60), 即夹角大于60度则返回
    //这个计算理论是怎么样的？
    // GetNormal()获得平均观测方向
    cv::Mat Pn = pMP->GetNormal();

    //P0点乘Pn,得到的应该是|Pn|*cos(PO,Pn)
    const float viewCos = PO.dot(Pn)/dist;
//其中viewingCosLimit是实参，值为0.5，刚好余弦的60度
    if(viewCos<viewingCosLimit)
        return false;

有一个疑惑，P0点乘Pn,得到的应该是|Pn|*cos(PO,Pn)但是为什么说是得到的是cos(PO,Pn)，并且直接和viewingCosLimit=0.5进行判断呢？？

根据深度预测尺度

这里是直接通过函数int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF)进行计算

对实参pMP添加属性

 // 标记该点将来要被投影
    pMP->mbTrackInView = true;
    pMP->mTrackProjX = u;
    pMP->mTrackProjXR = u - mbf*invz; //该3D点投影到双目右侧相机上的横坐标
    pMP->mTrackProjY = v;
    pMP->mnTrackScaleLevel = nPredictedLevel;
    pMP->mTrackViewCos = viewCos;

以上参数都是仅用与跟踪要的，时刻可能会被改变

void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)

函数简介

当前函数是基于RGBD相机的帧被创建的时候才会在构造函数中被调用，一直都是只有双目摄像机才会有右图像，这里是有深度信息和IR设备的基线（估计是摄像头到IR设备的距离），那么这样可以通过$d=\frac{f_{x}b}{Z}$得到视差，并通过视差和特征点像素横坐标可得到理论上的右图像对应的像素横坐标$u_r$ ，填充这个的目的是为了在进行G2O优化的时候，基于深度相机下，多一个约束（空间点的深度值也会根据视差公式进行转换得到上的$u_r$，然后和上一个作差），边的误差值也就变成vector3d类型。

知识点

深度值的提取和内参的理解

要明白像素平面是u做横坐标，v做纵坐标，所以在像素矩阵中应该是（v，u）的提取方式，第v行第u列

实际上像素平面就是相机坐标系的平移和缩放得到的，那么就有相应的像素坐标转换公式
$$\{\begin{array}{l}u=\alpha X^{\prime }+c_x\\ v=\beta Y^{\prime }+c_y\end{array}$$

其中$\alpha X^{\prime }$和$\beta Y^{\prime }$分别表示在x和y轴上的缩放，$X^{\prime }$和$Y^{\prime }$ 是物理成像平面的坐标，$c_x$和$c_y$分别表示平移，根据成像模型和三角形相似原理进行代入得到
$$\{\begin{array}{l}u=\alpha f\frac{X}{Z}+c_x\\ v=\beta f\frac{Y}{Z}+c_y\end{array}$$

X和Z都是像极坐标系的坐标值，那么$\alpha f$就成了焦距乘上了横坐标方向的缩放，变成了$f_x$并且单位是像素，相同的$\beta f$也是一样的，成为$f_y$ ，它们也都称为焦距，单位是像素，最终
$$\{\begin{array}{l}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_y\end{array}$$
可参考链接

存在一个疑问在程序中是直接用未被校正过的特征点的像素坐标在深度图中采集深度，为什么不是用校正过的像素坐标进行深度采集呢？？？

RGBD的Frame进行右图像横坐标填充

从深度图得到特征点的深度信息并且摄像头到IR设备的基线是已知的，那么理论上是可以得到IR位置图像的像素横坐标，所以这里的填充数据就是这个，这主要是为了在后面进行优化的时候多一个约束，误差类型就成了一个vector3d类型。
首先这里和双目相机计算深度信息的原理是一样的

根据三角形 $\Delta P{P}_L{P}_R$ 和$\Delta P{O}_L{O}_R$的相似关系,有:
$$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$$
整理得到
$$z=\frac{fb}{d}d=u_L-u_R$$
其中d就是所说的视差，左右图横坐标之差

如程序片段中：

mvuRight[i] = kpU.pt.x-mbf/d;

相当于公式
$$u_L-\frac{f_{x}b}{d}=u_R$$
其中b是双目相机的基线（这里是摄像头到IR设备的距离），d是该特征点的深度值（相当于Z坐标值），在配置文件中直接得到的是$f_{x}b$的乘积，不会直接给出基线b，因为b和d单位都是物理长度单位，相除就会约去，只剩下$f_x$的像素单位，符合等式。可参考证明