三角测量

在单目相机的初始化过程中，我们已经成功地根据匹配的特征点对，估计出了当前帧与参考帧之间的单应矩阵$\boldsymbol{H}$，或者基础矩阵$\boldsymbol{F}$。并且了解到从单应矩阵中可以找到8组可能的相机位姿，基础矩阵也可以分解出4组可能的相机位姿。类 Initializer 还会通过其成员函数 CheckRT 进一步检查这些解选取其中最优的那个。

函数 CheckRT 先假设各种可能的解都是对的，然后用其给出的旋转矩阵$\boldsymbol{R}$和平移向量$\boldsymbol{t}$，构造投影矩阵，完成对匹配的特征点对进行三角测量。选取三角测量成功最多的那个作为输出，然后类 Initializer 会根据成功三角测量的特征点数和视差进一步判定相机位姿的估计质量。

本文中，我们先来看一下三角测量的原理，给定相机位姿输出特征点的空间坐标，再具体分析被 CheckRT 调用的函数 Triangulate。然后介绍一下视差的概念，再结合函数 CheckRT 来查看如何计算视差。

1. 三角测量原理

现在假设相机的内参矩阵为$\boldsymbol{K}$，根据单应矩阵或者基础矩阵估计出相机的旋转矩阵$\boldsymbol{R}$和平移向量$\boldsymbol{t}$。那么对于空间中一点$\boldsymbol{X} = \begin{bmatrix} x & y & z \end{bmatrix}^T$，其在相机中的成像点$\boldsymbol{x} = \begin{bmatrix} u & v & 1 \end{bmatrix}^T$。那么我们可以写出3D坐标到2D像素之间的投影关系：

$$ \begin{equation}\label{f1} \boldsymbol{x} = \cfrac{1}{z} \boldsymbol{K} [\boldsymbol{R} | \boldsymbol{t}] \boldsymbol{X} \Longleftrightarrow \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \cfrac{1}{s} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \end{equation} $$

上式中符号$s = r_{31} x + r_{32} y + r_{33} z + t_z$为空间中的点$\boldsymbol{X}$在经 $[\boldsymbol{R} | \boldsymbol{t}]$变换之后得到的点到相机的距离。我们把矩阵$\boldsymbol{K} [\boldsymbol{R} | \boldsymbol{t}]$称为空间点到像素点的投影矩阵来表示。若参考帧的投影矩阵为$\boldsymbol{P}$，空间点$\boldsymbol{X}$在其中的投影像素为$\boldsymbol{x}$，那么其投影关系可以记为$\boldsymbol{x} = \frac{1}{s}\boldsymbol{PX}$。设当前帧的投影矩阵为$\boldsymbol{P'}$，空间点$\boldsymbol{X}$在其中的投影像素为$\boldsymbol{x'}$，那么其投影关系可以记为$\boldsymbol{x'} = \frac{1}{s'}\boldsymbol{P'X}$。现在我们的目标是要根据这两帧的像素点坐标来估计空间点$\boldsymbol{X}$的坐标。

ORB-SLAM2中采用了一种线性三角形的方法进行求解的，其思路与求解单应矩阵的DLT算法一样，也是将两个点的约束构建成$\boldsymbol{AX=0}$的形式，然后对矩阵$\boldsymbol{A}$进行SVD分解，取零空间的特征向量作为$\boldsymbol{X}$的估计。我们将像素点坐标叉乘到投影方程的两侧，可以去除掉尺度因子。

$$ \boldsymbol{x} \times \boldsymbol{PX} = \boldsymbol{0} \Rightarrow \begin{cases} (u P_3^T - P_1^T) \boldsymbol{X} = 0 \\ (v P_3^T - P_2^T) \boldsymbol{X} = 0 \\ (u P_2^T - vP_1^T) \boldsymbol{X} = 0 \\ \end{cases} , \boldsymbol{x'} \times \boldsymbol{P'X} = \boldsymbol{0} \Rightarrow \begin{cases} (u' {P'}_3^T - {P'}_1^T) \boldsymbol{X} = 0 \\ (v' {P'}_3^T - {P'}_2^T) \boldsymbol{X} = 0 \\ (u' {P'}_2^T - v'{P'}_1^T) \boldsymbol{X} = 0 \\ \end{cases} $$

上式中，$P_i$为投影矩阵中的第$i$行。从上式中每帧图像的像素点上取两个约束，就可以得到一个关于4个齐次坐标的4个方程$\boldsymbol{AX=0}$，其中

$$ \begin{equation} \label{f2} \boldsymbol{A} = \begin{bmatrix} u P_3^T - P_1^T \\ v P_3^T - P_2^T \\ u' {P'}_3^T - {P'}_1^T \\ v' {P'}_3^T - {P'}_2^T \end{bmatrix} \end{equation} $$

对上述矩阵$\boldsymbol{A}$进行SVD分解，取V矩阵的最后一列就是$\boldsymbol{X}$。因为在ORB-SLAM2中，地图坐标系是以初始化时的参考帧为基准构建的，所以参考帧的投影矩阵为$\boldsymbol{P} = \boldsymbol{K}[\boldsymbol{I} | \boldsymbol{0}]$。

2. 函数 Triangulate

下面是函数 Triangulate 的代码片段，该函数被 CheckRT 调用，用于构造矩阵$A$并对齐进行SVD分解。它有5个参数，分别用于输入参考帧和当前帧的特征点像素坐标，以及两帧的投影矩阵，输出分解得出的空间点坐标。我们将在分析函数 CheckRT 的时候看到投影矩阵的构造过程。

        void Initializer::Triangulate(const cv::KeyPoint &kp1,    // 参考帧中的特征点
                                      const cv::KeyPoint &kp2,    // 当前帧中的特征点
                                      const cv::Mat &P1,          // 参考帧的投影矩阵
                                      const cv::Mat &P2,          // 当前帧的投影矩阵
                                      cv::Mat &x3D) {             // 输出空间点坐标

然后根据式($\ref{f2}$)构造矩阵$A$:

            cv::Mat A(4,4,CV_32F);
            A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
            A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
            A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
            A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

接着调用OpenCV的接口完成SVD分解。第14行中，除以向量的最后一个元素是为了满足式($\ref{f1}$)，将分解出的向量最后一项置为1。

            cv::Mat u,w,vt;
            cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
            x3D = vt.row(3).t();
            x3D = x3D.rowRange(0,3)/x3D.at(3);
        }

3. 视差 Parallax

ORB-SLAM2通过视差(Parallax)来量化两帧图像之间的相对平移量。当视差较小时，三角测量的结果容易收到噪声的影响，导致最后的定位结果不准。所以 ORB-SLAM2 只选取具有足够视差的两帧图像进行初始化。

我查了一下维基百科，视差(Parallax)应该是从天文学中引入的一个词，被用来计算恒星到地球的距离。右侧是从该网页上找到的一张图，可以比较形象的讲述视差这个概念。图中，我们分别从 A, B 两个点观测中间那个黄色的四角星，在四角星背后从上到下有红、白、蓝三个色块。在 A 点看到四角星挡在了蓝色块的前面，B 点则在红色块的前面。视差就是我们从A,B两点观测四角星时的视线的夹角。

当被观测的物体到 AB 连线的距离固定的时候，视差越大意味着观测点 A 和 B 之间的平移量就越大。如果AB点之间的距离固定，那么视差越大物体就越近。

Parallax is a displacement or difference in the apparent position of an object viewed along two different lines of sight and is measured by the angle or half-angle of inclination between those two lines.

现在我们已经估计出了物体的空间坐标$\boldsymbol{\hat{X}} = \begin{bmatrix} \hat{x} & \hat{y} & \hat{z} \end{bmatrix}^T$。而且ORB-SLAM2认为参考帧的相机位于原点处，即$\boldsymbol{t_0} = \boldsymbol{0}$，而当前帧的相对于参考帧的平移量估计为$\boldsymbol{\hat{t}} = \begin{bmatrix} \hat{t_x} & \hat{t_y} & \hat{t_z} \end{bmatrix}^T$。那么视差角$\theta$可以通过两个向量的点积来求得：

$$ \begin{cases} \boldsymbol{l_0} = \hat{X} - \boldsymbol{t_0} \\ \boldsymbol{l_1} = \hat{X} - \boldsymbol{\hat{t}} \\ \cos \theta = \frac{\boldsymbol{l_0} \cdot \boldsymbol{l_1}}{\| \boldsymbol{l_0} \| \| \boldsymbol{l_1} \|} \end{cases} $$

4. 函数 CheckRT

初始化器 Initializer 通过其成员函数 CheckRT 来检验估计的相机姿态矩阵和平移向量的准确性。该函数对匹配点对进行三角测量，筛选出其中同时出现在两帧图像的相机前方并且具有一定视差的点，作为成功定位的点。如果位姿估计的准确，那么绝大部分的匹配点都应该出现在两帧相机的前面。

下面是函数 CheckRT 的代码片段，它的参数有点点多，最后返回的是成功进行三角测量的特征点数量。其中输入参数 R, t 分别为当前帧相机的姿态估计值，vKeys1, vKeys2 分别记录了参考帧与当前帧的特征点列表。 vMatches12 和 vbMatchesInliers 输入了两帧之间的特征点匹配关系并指示了匹配点对是否为内点，K是相机矩阵，th2是重投影误差的阈值。vP3D 输出了三角测量之后的空间点坐标， vbGood 与参考帧中的特征点一一对应标记着它们是否成功定位了，parallax输出了两帧相机之间的视差角估计值。

        
		int Initializer::CheckRT(const cv::Mat &R,                   // 当前帧相机姿态矩阵的估计值
 								 const cv::Mat &t,                   // 当前帧相机平移向量的估计值
								 const vector<cv::KeyPoint> &vKeys1, // 参考帧的特征点列表
								 const vector<cv::KeyPoint> &vKeys2, // 当前帧的特征点列表
                       			 const vector<Match> &vMatches12,    // 当前帧与参考帧中特征点的匹配关系
								 vector<bool> &vbMatchesInliers,     // 匹配的特征点是否合理的标记
                       			 const cv::Mat &K,                   // 相机内参矩阵
								 vector<cv::Point3f> &vP3D,          // 三角测量输出的空间点坐标
								 float th2,                          // 重投影误差的阈值
							     vector<bool> &vbGood,               // 对应参考帧中特征点是否已经成功进行三角测量
								 float &parallax) {                  // 视差

首先先从相机内参矩阵中取出两轴的焦距 fx, fy 和光心坐标 cx, cy。再为输出参数申请内存，并创建局部变量 vCosParallax 记录各点的视差角余弦值。

            const float fx = K.at<float>(0,0); const float fy = K.at<float>(1,1);
            const float cx = K.at<float>(0,2); const float cy = K.at<float>(1,2);
            vbGood = vector(vKeys1.size(),false);
            vP3D.resize(vKeys1.size());
            vector vCosParallax;
            vCosParallax.reserve(vKeys1.size());

然后按照式($\ref{f1}$)构建投影矩阵P1,P2，以及相机的空间坐标O1,O2。参考帧的投影矩阵为$\boldsymbol{P1} = \boldsymbol{K}[\boldsymbol{I} | \boldsymbol{0}]$，当前帧的投影矩阵为$\boldsymbol{P2} = \boldsymbol{K}[\boldsymbol{R} | \boldsymbol{t}]$。

            cv::Mat P1(3,4,CV_32F,cv::Scalar(0));
            K.copyTo(P1.rowRange(0,3).colRange(0,3));

            cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);

            cv::Mat P2(3,4,CV_32F);
            R.copyTo(P2.rowRange(0,3).colRange(0,3));
            t.copyTo(P2.rowRange(0,3).col(3));
            P2 = K*P2;
            cv::Mat O2 = -R.t()*t;

接下来在一个 for 循环中遍历所有的匹配点对。

            int nGood=0;
            for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) {
                if (!vbMatchesInliers[i])
                    continue;

通过函数 Triangulate 对匹配点对进行三角测量。

                const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
                const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
                cv::Mat p3dC1;
                Triangulate(kp1,kp2,P1,P2,p3dC1);
                if (!isfinite(p3dC1.at<float>(0)) ||
                    !isfinite(p3dC1.at<float>(1)) ||
                    !isfinite(p3dC1.at<float>(2))) {
                    vbGood[vMatches12[i].first]=false;
                    continue;
                }

通过向量的内积估计视差角的余弦值。

                cv::Mat normal1 = p3dC1 - O1;
                float dist1 = cv::norm(normal1);
                cv::Mat normal2 = p3dC1 - O2;
                float dist2 = cv::norm(normal2);
                float cosParallax = normal1.dot(normal2)/(dist1*dist2);

由于参考帧的相机位于地图的原点，所以估计出的空间点坐标的 z 轴分量就是其到参考帧的相机距离，该值大于零说明空间点在参考帧的前方。下面的 if 语句将抛弃掉那些不再参考帧前方的点。它的第二个条件有点奇怪，感觉是个bug，因为在区间$[0, \pi]$上余弦关于角度是单调递减的。也就是说视差越大其余弦值越小，所以个人认为这个条件应该是cosParallax > 0.99998。

                if (p3dC1.at<float>(2) <=0 && cosParallax < 0.99998)
                    continue;

然后根据相机位姿，将 p3dC1 的空间点坐标转换到当前帧的坐标系下，再次检查空间点是否出现在当前帧的前方。第二个条件有点多余，还有bug。

                cv::Mat p3dC2 = R*p3dC1+t;
                if (p3dC2.at<float>(2) <= 0 && cosParallax < 0.99998)
                    continue;

紧接着，把空间点坐标投影到图像上，计算投影后的像素坐标与原特征点的坐标之间的欧氏距离，即重投影误差，抛弃掉那些误差比较大的点。

                float im1x, im1y;
                float invZ1 = 1.0 / p3dC1.at<float>(2);
                im1x = fx * p3dC1.at<float>(0) * invZ1 + cx;
                im1y = fy * p3dC1.at<float>(1) * invZ1 + cy;
                float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
                if (squareError1 > th2)
                    continue;

类似的操作，在当前帧上再来一遍。

                float im2x, im2y;
                float invZ2 = 1.0 / p3dC2.at<float>(2);
                im2x = fx * p3dC2.at<float>(0) * invZ2 + cx;
                im2y = fy * p3dC2.at<float>(1) * invZ2 + cy;
                float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
                if (squareError2 > th2)
                    continue;

通过了上面的层层考验之后，CheckRT 认为对于一个匹配点对的三角测量是准确的，将其结果和视差值记录下来，并增加计数 nGood。第65行中的 if 语句的存在不合理。

                vCosParallax.push_back(cosParallax);
                vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
                nGood++;
                if (cosParallax < 0.99998)
                    vbGood[vMatches12[i].first]=true;
            }

最后，根据视差对特征点排序，为了防止测量噪声的影响，选取了其中较大的一个视差(第50个特征点)作为两帧间平移量的评价，并返回计数 nGood。

            if (nGood > 0) {
                sort(vCosParallax.begin(),vCosParallax.end());
                size_t idx = min(50,int(vCosParallax.size()-1));
                parallax = acos(vCosParallax[idx])*180/CV_PI;
            } else
                parallax=0;
            return nGood;
        }

5. 完

本文中，我们了解了三角测量的原理和SVD求解空间点坐标的方法，也查看了ORB-SLAM2中三角测量的实现函数 Triangulate。然后介绍了视差的概念，最后分析了函数 CheckRT。在该函数中我们看到了视差和重投影误差的计算方法。在分析代码时，发现其视差的判定条件可能是一个bug。