针对双目和深度相机的调整(TRACKING)

2015年发布的 ORB-SLAM 只是一个单目的视觉SLAM系统，在2017年发布的 ORB-SLAM2 支持了双目和深度相机。下面左右两张图是分别从15年和17年论文中抠出来的关于轨迹跟踪过程的框图，两者略有不同。

相比于单目而言，双目和深度相机的预处理过程要复杂一点。在计算了 ORB 特征点之后，它们还需要进一步根据左右目图像或者深度图像，计算特征点的深度和空间坐标。图中 Extract ORB 变成了 Pre-process Input。双目和深度相机，不需要单目相机那样复杂的初始化过程，所以在 ORB-SLAM2 的 TRACKING 框图中就没有体现地图初始化的过程(Map Initialization)。但是建立地图坐标系的过程是必不可少的。相比与单目相机，双目和深度相机的特征点可以提供更多的信息，所以通过图优化估计相机位姿时建立的约束也不一样。

本文中，我们将详细介绍这三个环节的调整。剩下的两个环节，根据局部地图优化位姿、生成关键帧，的过程差异并不大，只有一些特定的参数不同，多了一点特别处理流程而已，本文不再详细展开。


(a). ORB-SLAM 的 TRACKING 系统框图	(b). ORB-SLAM2 的 TRACKING 系统框图

1. 图像的预处理

如右图所示，双目和深度相机输出的图像经过预处理之后，得到 Stereo 和 Monocular 两类特征点。 Stereo 特征点是指那些在左右目图像中成功匹配的特征点，或者是在深度图中有合法数值的特征点。它们可以通过相机内参直接输出左目坐标系下的点坐标。 Monocular 特征点是指左目图像中没有成功匹配的特征点，或者是深度图数值非法的特征点。它们需要多次观测通过三角化的方式计算点坐标。

针对双目相机，我们分别提取左右目图像的特征点。然后对于左目图像的所有特征点都在右目图像中进行匹配。对于那些成功匹配的左目特征点用$\left(u_L, v_L, u_R\right)$表示。其中，$\left(u_L, v_L\right)$是特征点在左目图像中的像素坐标，$u_R$是右目图像的横轴坐标。对于深度相机，首先提取图像的特征点，然后对应地在深度图中查找其深度值，构建一个虚拟的双目相机，按照下式计算特征点在虚拟的右目图像中的横轴坐标:

$$ u_R = u_L - \frac{f_x b}{d} $$

其中，$f_x$是深度相机横轴的焦距，$b$是虚拟的基线长度，$d$是深度值。所谓的基线是指左右目相机的间距，原文中设定的虚拟基线是 8cm。之后深度相机就被看做是双目相机使用，它们的处理方式都是一样的。

此外，由双目和深度相机原理决定的，远处的视差/深度值测量误差较大。所以 ORB-SLAM2 以40倍基线长度为阈值(40 times the stere/RGB-D baseline)，将 Stereo 特征点分成了近(close)、远(far)两种形式。近处的特征点可以根据一帧的传感器数据估计出其空间位置，提供相机的尺度、平移和旋转的信息。远处的特征点处理有点类似 Monocular 特征点，需要多帧数据完成空间坐标的估计，并且在做优化的时候，也主要是提供相机的旋转信息，尺度和平移的权重较小。

相关的预处理过程，主要体现在类 Frame 的构造函数中。下面的代码片段分别是根据双目和深度相机构造 Frame 的函数，我们省略了一些语句，并对关键的语句做了一些注释，这里不再详细展开。

展开根据双目相机构造类 Frame >>>

        Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp,
                     ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K,
                     cv::Mat &distCoef, const float &bf, const float &thDepth) : // 省略初值列表
        {
            // 省略记录图像金字塔参数

            // 分别在两个线程中提取左右目图像特征点
            thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
            thread threadRight(&Frame::ExtractORB,this,1,imRight);
            threadLeft.join();
            threadRight.join();

            // mvKeys 记录的是左目图像的特征点
            N = mvKeys.size();
            if(mvKeys.empty())
                return;

            // 校畸变、匹配双目的特征点
            UndistortKeyPoints();
            ComputeStereoMatches();

            // 省略其它成员变量的初始化
        }

展开根据深度相机构造类 Frame >>>

        Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp,
                     ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K,
                     cv::Mat &distCoef, const float &bf, const float &thDepth) : // 省略初值列表
        {
            // 省略记录图像金字塔参数

            // 提取特征点
            ExtractORB(0,imGray);
            N = mvKeys.size();
            if(mvKeys.empty())
                return;

            // 校畸变、构造虚拟双目相机
            UndistortKeyPoints();
            ComputeStereoFromRGBD(imDepth);

            // 省略其它成员变量的初始化
        }

2. 系统初始化过程

对于双目和深度相机而言，由于不需要确定基础尺度，可以直接依据深度信息，来估计特征点在3D世界中的位姿，并完成地图的初始构建工作。

在帧间位姿估计和地图初始化总体过程分析地图初始化的总体过程时，我们已经看到双目和深度相机都是通过函数 StereoInitialization 完成初始化的。下面是该函数的代码片段。首先检查当前帧的特征点数量，如果太少就放弃了。然后将当前帧的姿态设定到原点上，并以此构建关键帧添加到地图对象mpMap中。可见因为深度信息的存在，我们只需要一帧图像就可以完成初始化。

        void Tracking::StereoInitialization() {
            if (mCurrentFrame.N <= 500)
                return;
            mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
            mpMap->AddKeyFrame(pKFini);

然后，检查当前帧的特征点，如果其深度值大于零，就根据深度信息和相机内参，通过函数 UnprojectStereo 得到特征点在相机坐标系下的 3D 坐标。并据此新建地图点将之与关键帧关联上。

展开遍历特征点构建地图点 >>>

            for(int i = 0; i < mCurrentFrame.N; i++) {
                float z = mCurrentFrame.mvDepth[i];
                if (z > 0) {
                    cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                    MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
                    pNewMP->AddObservation(pKFini,i);
                    pKFini->AddMapPoint(pNewMP,i);
                    pNewMP->ComputeDistinctiveDescriptors();
                    pNewMP->UpdateNormalAndDepth();
                    mpMap->AddMapPoint(pNewMP);

                    mCurrentFrame.mvpMapPoints[i]=pNewMP;
                }
            }
            cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;

此时就已经完成了地图的初始构建工作，接下来就是更新系统的相关变量和状态。

展开更新系统的相关变量和状态 >>>

            mpLocalMapper->InsertKeyFrame(pKFini);

            mLastFrame = Frame(mCurrentFrame);
            mnLastKeyFrameId=mCurrentFrame.mnId;
            mpLastKeyFrame = pKFini;
        
            mvpLocalKeyFrames.push_back(pKFini);
            mvpLocalMapPoints=mpMap->GetAllMapPoints();
            mpReferenceKF = pKFini;
            mCurrentFrame.mpReferenceKF = pKFini;
        
            mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
            mpMap->mvpKeyFrameOrigins.push_back(pKFini);
            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
        
            mState=OK;
        }

3. 相机位姿的初始估计

完成 SLAM 系统的初始化工作，建立地图坐标系之后，ORB-SLAM2 将估计每一帧图像的相机在该坐标系下的位姿。一般情况下ORB-SLAM都是先通过匀速运动模型粗略的估计相机位姿，再通过特征点匹配和优化方法给出一个较为精确的位姿估计。有时相机可能运动过快，导致里程计跟丢了，此时ORB-SLAM会通过词袋模型，以及特征点匹配来重定位。

这部分工作的差异主要体现在，特征点匹配之后的优化过程中，既函数 Optimizer::PoseOptimization 的实现相比于单目相机要复杂那么一点。主要是因为，双目和深度相机的每帧数据提取出的特征点被分为了 Stereo 和 Monocular 两类，而且 Stereo 类的特征点还有远近点的概念。针对不同类别的特征点建立的约束也多少有些差异。

我们在匀速运动模型估计相机位姿中，详细分析了函数 Optimizer::PoseOptimization 的实现，但是省略了关于 Stereo 类的特征点的约束。该函数将各个特征点的像素坐标和空间坐标所构成的约束，描述成一个图的结构。这个图中，只有一个节点，就是待估计的相机位姿。下面左侧是该函数的代码片段，其中定义了容器 vpEdgesStereo 来保存各个 Stereo 类型的特征点构建的约束对象，本文以后简称双目边。vnIndexEdgeStereo 记录各个双目边的索引。最后的局部变量 deltaStereo 应该是一个自由度为 3，显著性水平为 0.05 的卡方阈值，它将用于优化求解器的 Huber 核函数。

        vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
        vector<size_t> vnIndexEdgeStereo;
        vpEdgesStereo.reserve(N);
        vnIndexEdgeStereo.reserve(N);
        // 看样子是自由度为3，显著性水平为0.05的卡方阈值。
        const float deltaStereo = sqrt(7.815);

        nInitialCorrespondences++;
        pFrame->mvbOutlier[i] = false;
        Eigen::Matrix<double,3,1> obs;
        const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
        const float &kp_ur = pFrame->mvuRight[i];
        obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

上面右侧的代码片段，是在一个 for 循环中遍历当前帧的所有 stereo 类型的特征点。变量 nInitialCorrespondences 是特征点的计数， obs 是根据特征点在左目的像素坐标和右目的像素横坐标，构建的观测量。下面左侧的代码片段是在构建 EdgeStereoSE3ProjectXYZOnlyPose 类型的边，该类型内置了误差函数的计算。通过接口 setVertex 将边的两端都指向图中唯一的节点，表示相机位姿估计的约束。接口 setMeasurement 设置了边的观测值，接口 setInformation 描述了边的信息矩阵。填充完边的相关字段之后，就可以按照下面右侧的代码片段所示那样，将之添加到图中参与后续的优化。

        g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();
        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        e->setMeasurement(obs);
        const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
        Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
        e->setInformation(Info);

        // 省略其它关于边的赋值语句
        optimizer.addEdge(e);
        vpEdgesStereo.push_back(e);
        vnIndexEdgeStereo.push_back(i);

4. 完

本文中，我们首先分析了双目和深度相机的图像预处理过程，它将提取的特征点分为 Stereo 和 Monocular 两类。此外，为了统一深度相机和双目相机的操作，ORB-SLAM2 将深度相机模拟成双目相机。由于双目和深度相机可以直接估计特征点在相机坐标系下的位置，所以其初始化过程就没有单目那么麻烦。ORB-SLAM2 针对 Stereo 和 Monocular 两类特征点，构建了不同的约束边进行优化。