ORB_SLAM2原理及代码解析：Frame::UnprojectStereo() 函数

1 作用

把当前帧中的第 i 个特征点反投影到世界坐标系（如果有深度信息）。

2 所属位置及代码

（1）声明：include/Frame.h

    // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.cv::Mat UnprojectStereo(const int &i);

（2）定义：src/Frame.cc

cv::Mat Frame::UnprojectStereo(const int &i)
{const float z = mvDepth[i];if(z>0){const float u = mvKeysUn[i].pt.x;const float v = mvKeysUn[i].pt.y;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);return mRwc*x3Dc+mOw;}elsereturn cv::Mat();
}

3 原理解析

思路：像素坐标-去畸变-归一化-相机坐标-世界坐标

mvDepth[i]：存储第 i 个特征点的深度；

mvKeysUn[i]：去畸变后的关键点；

(u,v)：是图像平面上的像素坐标

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

ORB_SLAM2中在ORB提取特征点时已经“去畸变”，得到的是去畸变后的像素坐标。

3.2 const float x = (u-cx)*z*invfx;
const float y = (v-cy)*z*invfy;

去畸变后的像素坐标转化为相机坐标。

3.3 cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);

把反投影（像素坐标-相机坐标）得到的相机坐标系下的三维点构造成 OpenCV 的 cv::Mat 矩阵。

3.4 return mRwc*x3Dc+mOw;

输出世界坐标系下的坐标。注意相机初始化是用的是左目的数据。

3.5 公式推导