前言

这个函数是在跟踪线程中更新上一帧的函数中被调用。

1.函数声明

cv::Mat Frame::UnprojectStereo(const int &i)

2.函数定义 

1.获取这个特征点的深度值。

const float z = mvDepth[i];

深度值由双目或 RGB-D 传感器获取。

在双目情况下，这个深度来自视差ComputeStereoMatches。

在RGB-D情况下，这个深度来自深度图对应像素的测量。

2.只有在深度值满足大于0的条件下才能进行下一步，否则直接返回一个空矩阵。

if(z>0)
    {
        ....
    }
else
    return cv::Mat();

3.获取矫正后的特征点的像素坐标。

const float u = mvKeysUn[i].pt.x;
const float v = mvKeysUn[i].pt.y;

 4.将当前像素坐标反投影到相机坐标系下。

const float x = (u - cx) * z * invfx;
const float y = (v - cy) * z * invfy;
cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);

反投影公式：

5.返回到世界坐标系下。

return mRwc * x3Dc + mOw;

mRwc 表示从相机坐标系到世界坐标系的旋转矩阵。

mOw 表示相机在世界坐标系下的平移（通常是世界坐标下相机光心的位置）。

结束语 

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