计算相似变换SE3

在计算候选闭环关键帧中， ORB-SLAM2 根据词袋模型和一致共视组(ConsistentGroup)的概念筛选出了一些可能存在闭环的关键帧。自然地，接下来的工作就是要估计这些候选关键帧与当前帧之间的位姿关系， 建立几何约束进一步对它们筛选。由于单目相机存在尺度不确定的问题，除了需要估计三轴平移和三轴旋转之外，还需要估计尺度，也就是所谓的相似变换(SE3)。

闭环探测的总体过程告诉我们，LoopClosing 通过成员函数 ComputeSim3 完成相似变换的估计和基于几何约束的筛选。 其整体过程可以分为四个部分：① 为每一个候选帧构建一个 Sim3Solver 求解器；② 以 RANSAC 的方式初步求解各个候选帧的 Sim3，并通过投影匹配筛选内点。③ 当有一个候选帧有超过20个的内点时， 停止遍历，并将之认定为闭环帧更新地图。④ 最后还会对闭环帧的共视组中所有的地图点进行一次投影匹配，得到不少于40个匹配地图点，才认定发生了一次闭环。

本文中，我们先详细分析 Sim3Solver 核心算法的原理和实现上， 再简单过一遍 LoopClosing 的成员函数 ComputeSim3 的业务逻辑。

1. 问题描述和拆解

假设我们分别在左右两个坐标系下观测到了 \(n\) 个点，现在我们需要根据这些点的笛卡尔坐标估计两个坐标系之间的变换关系和尺度关系。 我们知道确定刚体的空间位置需要6个自由度，其中三个用于描述位置，另外三个用于描述姿态。 除此之外，单目相机还存在尺度的问题，对于两帧图像，我们还需要估计它们的尺度关系。所以我们要求解的是一个 7 自由度的问题。 为了方便，后文中将称之为 SE3 问题。

我们分别用集合 \(\{\boldsymbol{r}_{l, i}\}\) 和 \(\{\boldsymbol{r}_{l, i}\}\) 表示这 \(n\) 点在左右两个坐标系下的位置矢量，其中 \(i = 1 \cdots n\)。 那么 SE3 问题就是在寻找一组解 \(\{s, R(\bullet), \boldsymbol{r_0} \}\)，使得左坐标系下的点可以通过下式一一映射到右坐标系下： $$ \begin{equation}\label{se3_func} \boldsymbol{r}_r = sR(\boldsymbol{r}_l) + \boldsymbol{r_0} \end{equation} $$ 其中，\(s\)为尺度因子，\(\boldsymbol{r_0}\)为两个坐标系的平移向量，\(R(\bullet)\)则是一个旋转变换，满足关系\(\| R(\boldsymbol{r}_l) \|^2 = \| \boldsymbol{r}_l\|^2 \)。 由于空间中的每个点都有3个自由度，理论上三个不共线的点就可以提供 9 个约束，所以只需要三个点就可以求解 SE3 问题。但是测量总是存在误差的，仅凭三个点通常得不到精确的结果。 很多时候我们都是通过大量的观测样本，求得一个使损失函数最小化的解。 ORB-SLAM2 就是通过 horn 算法配合 RANSAC 实现的。

根据式\((\ref{se3_func})\)，我们可以写出残差公式:

还有损失函数：

1.1 求解平移向量

Horn 通过质心从损失函数\((\ref{loss_func})\) 中分解出了平移向量\(\boldsymbol{r}_0\)。左右两个坐标系下点的质心可以分别写成如下的左右两个式子：

令，\(\boldsymbol{r}_{l,i}' = \boldsymbol{r}_{l,i} - \bar{\boldsymbol{r}_l}, \boldsymbol{r}_{r,i}' = \boldsymbol{r}_{r,i} - \bar{\boldsymbol{r}_r} \)，有：

残差公式\((\ref{residual_error})\)可以改写为：

那么损失函数\((\ref{loss_func})\)可以展开为：

因为式\((\ref{f1})\)，所以上式的中间项为0。第一项与\(\boldsymbol{r}_0'\)无关，最后一项显然是个非负数。 如果上式能够取得最小值，那么最后一项应当为 0，即 \(\boldsymbol{r}_0'\)，因此我们可以写出平移向量\(\boldsymbol{r}_0\)的表达式：

现在只要估计出了尺度因子 \(s\) 和旋转变换 \(R\) 我们就可以求得\(\boldsymbol{r}_0\)。下面我们先把尺度因子给分解出来。

1.2 求解尺度因子

对于尺度因子而言，有一个重要的约束，就是需要保证 SE3 是对称的。也就是说，对于公式\((\ref{se3_func})\)而言，解\(\{s, R(\bullet), \boldsymbol{r_0} \}\)应当能够保证下式也是成立的：

在式\((\ref{translate_func})\)成立的情况下，损失函数可以简化为 \(\sum_{i=1}^n \| \boldsymbol{r}_{r,i}' - sR(\boldsymbol{r}_{l,i}') \|^2\)。 此时，我们可以把残差公式改写为 \(\boldsymbol{e}_i = \boldsymbol{r}_{r,i}' - sR(\boldsymbol{r}_{l,i}')\)。 如果针对式\((\ref{se3_func_2})\)重复第 1.1 节的推到过程，或者交换一下左右两个坐标系的身份，整理一下。 应该能够得到一个形如 \(\boldsymbol{e}_i = -\left[\frac{1}{s}\boldsymbol{r}_{r,i}' - R(\boldsymbol{r}_{l,i}')\right]\) 的残差。为了保证 SE3 的对称性， Horn 综合这两种形式，写出了如下的残差公式:我并没有纠结他是怎么综合的。

对应的，损失函数可以写成如下形式，并展开：

令，\(S_l = \sum_{i=1}^n \| \boldsymbol{r}_{l,i}' \|^2, D = \sum_{i=1}^n \boldsymbol{r}_{r,i}' \bullet R(\boldsymbol{r}_{l,i}'), S_r = \sum_{i=1}^n \| \boldsymbol{r}_{r,i}'\|^2\)，有：

上式中，第一项是个非负数，第二项与尺度因子 \(s\) 无关。所以上式取得最小值的时候，第一项一定为 0。因此可以求得尺度因子 \(s = \sqrt{(S_r / S_l)}\)，即：

上式允许我们在没有求得旋转变换的时候，计算尺度因子。而旋转变换则完全保留 \(D\) 中，最小化 \((\ref{loss_func_2})\) 相当于求得旋转变换 \(R(\bullet)\) 使得 \(D\) 最大化。 下面我们来研究一下问题的关键——如何求解旋转变换。

1.3 求解旋转变换

现在的问题就变成了寻找一个旋转变换 \(R(\bullet)\) 使得 \(D = \sum_{i=1}^n \boldsymbol{r}_{r,i}' \bullet R(\boldsymbol{r}_{l,i}')\) 最大化。 Horn 算法采用四元素表示旋转， 构建了一个矩阵\(N\)，该矩阵最大的特征值所对应的特征向量，就是能使 \(D\) 最大化的四元数。 我们曾在定点转动与四元数一文中，通过定点转动推导出罗德里格斯公式，并进一步的介绍了如何通过四元数表示旋转。 令\(\vec{q},\vec{r}\)分别表示四元数形式的旋转变换和样本点坐标，我们可以将优化目标函数写成如下的形式：

根据四元数的运算性质，\((\vec{p} \otimes \vec{q}) \bullet \vec{r} = \vec{p} \bullet (\vec{r} \otimes \vec{q}^*)\)，有

于笛卡尔坐标系下的一个空间点\(\boldsymbol{r} = (r_x, r_y, r_z)^T\)，可以写成四元数的形式，\(\vec{r} = 0 + r_x \vec{i} + r_y \vec{j} + r_z \vec{k}\)。 上式中的\(\bar{R}, R\)分别是四元数\(\vec{r}\)的右乘和左乘形式的矩阵：

我们将笛卡尔坐标系下的样本点排列成矩阵的形式, \(\boldsymbol{R}_l' = \begin{bmatrix} \boldsymbol{r}_{l,1}' \cdots \boldsymbol{r}_{l, n}' \end{bmatrix}\)， \(\boldsymbol{R}_r' = \begin{bmatrix} \boldsymbol{r}_{r,1}' \cdots \boldsymbol{r}_{r, n}' \end{bmatrix}\)，有:

其中，\(S_{xx} = \sum_{i=1}^n x_{l,i}' x_{r,i}', S_{xy} = \sum_{i=1}^n x_{l,i}'y_{r,i}', \cdots\)，矩阵\(N\)可以写成:

Horn 在论文的附录中， 证明了矩阵\(N\)的最大特征值所对应的特征向量就是能使 \(\vec{q}^T N \vec{q}\) 最大化的单位四元数。

2. SE3 求解的核心代码

根据上述 SE3 问题的拆解过程分析，我们拿到左右两帧图像的对应特征点坐标，可以通过如下的几个步骤分解出 SE3 变换：

1. 通过样本坐标构建矩阵 \(N\)；
2. 对矩阵 \(n\) 特征值分解，找到最大特征值对应的特征向量，得到四元数\(\vec{q}\)，构建旋转矩阵 \(R\)；
3. 套用公式\((\ref{scale_factor})\)计算尺度因子 \(s\)
4. 将旋转矩阵 \(R\) 和尺度因子 \(s\) 代入公式\((\ref{translate_func})\)中求得平移向量\(\boldsymbol{r}_0\)

下面是 Sim3Solver 的成员函数 ComputeSim3 的代码片段，它的输入参数 P1 和 P2 是两个 3x3 的矩阵，以列向量的形式分别记录着，当前帧和候选帧中匹配的三个地图点坐标。 该函数定义了局部变量 O1 和 O2 分别记录了 P1 和 P2 的质心，Pr1 和 Pr2 分别记录了去质心之后的地图点坐标。调用函数 ComputeCentroid 完成质心的计算和去质心的操作。

        void Sim3Solver::ComputeSim3(cv::Mat & P1, cv::Mat & P2) {
            cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)
            cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)
            cv::Mat O1(3,1,Pr1.type());       // Centroid of P1
            cv::Mat O2(3,1,Pr2.type());       // Centroid of P2
            ComputeCentroid(P1,Pr1,O1);
            ComputeCentroid(P2,Pr2,O2);

接下来，构建矩阵 \(N\)。在代码中，后缀 1 对应着右坐标系，后缀 2 对应着左坐标系。

            cv::Mat M = Pr2*Pr1.t();
            double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;
            cv::Mat N(4,4,P1.type());
            N11 =  M.at<float>(0,0) + M.at<float>(1,1) + M.at<float>(2,2);   // Sxx+Syy+Szz
            N12 =  M.at<float>(1,2) - M.at<float>(2,1);                    // Syz-Szy
            N13 =  M.at<float>(2,0) - M.at<float>(0,2);                    // Szx-Sxz
            N14 =  M.at<float>(0,1) - M.at<float>(1,0);                    // ...
            N22 =  M.at<float>(0,0) - M.at<float>(1,1) - M.at<float>(2,2);
            N23 =  M.at<float>(0,1) + M.at<float>(1,0);
            N24 =  M.at<float>(2,0) + M.at<float>(0,2);
            N33 = -M.at<float>(0,0) + M.at<float>(1,1) - M.at<float>(2,2);
            N34 =  M.at<float>(1,2) + M.at<float>(2,1);
            N44 = -M.at<float>(0,0) - M.at<float>(1,1) + M.at<float>(2,2);

            N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,
                                         N12, N22, N23, N24,
                                         N13, N23, N33, N34,
                                         N14, N24, N34, N44);

然后，计算矩阵 \(N\) 的特征值，取其中最大的那个对应的特征向量，作为四元数\(\vec{q}\)。并计算角轴矢量，最后通过罗德里格斯公式获取旋转矩阵。

            cv::Mat eval, evec;
            cv::eigen(N,eval,evec); // evec[0] is the quaternion of the desired rotation
            
            cv::Mat vec(1,3,evec.type());
            (evec.row(0).colRange(1,4)).copyTo(vec); // extract imaginary part of the quaternion (sin*axis)
            double ang=atan2(norm(vec),evec.at(0,0));
            vec = 2*ang*vec/norm(vec);
            mR12i.create(3,3,P1.type());

            cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis

接着，计算尺度因子\(s\)。代码中并不是按照公式(\(\ref{scale_factor}\))实现的对称的尺度因子。而是按照如下的形式计算的，只是这种实现方式得到的尺度因子不一定是对称的。

            cv::Mat P3 = mR12i*Pr2;
            if(!mbFixScale) {
                //! 单目相机需要估计尺度因子
                double nom = Pr1.dot(P3);
                cv::Mat aux_P3(P3.size(),P3.type());
                aux_P3=P3;
                cv::pow(P3,2,aux_P3);
                double den = 0;

                for(int i=0; i < aux_P3.rows; i++)
                    for(int j=0; j < aux_P3.cols; j++)
                        den+=aux_P3.at<float>(i,j);
                ms12i = nom/den;
            } else {
                //! 双目和深度相机不存在尺度因子的问题
                ms12i = 1.0f;
            }

最后，根据公式(\(\ref{translate_func}\))计算平移向量。

            mt12i.create(1,3,P1.type());
            mt12i = O1 - ms12i*mR12i*O2;

将尺度因子、旋转矩阵、平移向量组合成最后的 4x4 的变换矩阵。

            mT12i = cv::Mat::eye(4,4,P1.type());
            cv::Mat sR = ms12i*mR12i;
            sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));
            mt12i.copyTo(mT12i.rowRange(0,3).col(3));

            mT21i = cv::Mat::eye(4,4,P1.type());
            cv::Mat sRinv = (1.0/ms12i)*mR12i.t();
            sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));
            cv::Mat tinv = -sRinv*mt12i;
            tinv.copyTo(mT21i.rowRange(0,3).col(3));

3. 筛选候选关键帧

LoopClosing 通过成员函数 ComputeSim3 完成相似变换的估计和基于几何约束的筛选。其整体过程可以分为四个部分：

1. 为每一个候选帧构建一个 Sim3Solver 求解器；
2. 以 RANSAC 的方式初步求解各个候选帧的 Sim3，并通过投影匹配筛选内点；
3. 当有一个候选帧有超过20个的内点时，停止遍历，并将之认定为闭环帧；
4. 最后还会对闭环帧的共视组中所有的地图点进行一次投影匹配，得到不少于40个匹配地图点，才认定发生了一次闭环。

下面是 LoopClosing 的成员函数 ComputeSim3 的代码片段。其中，容器 mvpEnoughConsistentCandidates 中记录的是通过一致连续性检查的候选闭环关键帧。 局部容器 vpSim3Solvers,vvpMapPointMatches,vbDiscarded 与候选关键帧一一对应，分别用于，保存将要构造的求解器，记录候选帧与当前帧的匹配地图点，标记是否抛弃候选帧。

        bool LoopClosing::ComputeSim3() {
            const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
            ORBmatcher matcher(0.75,true);
            vector<Sim3Solver*> vpSim3Solvers;            vpSim3Solvers.resize(nInitialCandidates);
            vector<vector<MapPoint*>> vvpMapPointMatches; vvpMapPointMatches.resize(nInitialCandidates);
            vector<bool> vbDiscarded;                     vbDiscarded.resize(nInitialCandidates);

完成准备工作之后，在一个 for 循环中遍历所有的候选帧，并在其中第 15 行中通过词袋模型进行初次特征点匹配，抛弃掉那些匹配点少于 20 个的候选帧。在 21-22 行为剩下的候选帧构建求解器。 其中，局部变量 nCandidates 统计了有效的候选帧数量。

            int nCandidates=0;
            for(int i = 0; i < nInitialCandidates; i++) {
                KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
                pKF->SetNotErase();
                if(pKF->isBad()) {
                    vbDiscarded[i] = true;
                    continue;
                }
                int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
                if(nmatches<20) {
                    vbDiscarded[i] = true;
                    continue;
                }
                Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
                pSolver->SetRansacParameters(0.99,20,300);
                vpSim3Solvers[i] = pSolver;
                nCandidates++;
            }

接着再次遍历所有的候选帧，在下面的第33行，通过求解器以RANSAC的方式迭代5次。局部变量 bNoMore 标识着是否求解成功，若成功将得到当前帧到候选帧的相似变换 Scm。 在这次遍历过程中，还会对那些成功求解的候选帧，再次通过投影匹配筛选匹配特征点，并对相似变换进行优化。

            for(int i = 0; i < nInitialCandidates; i++) {
                if(vbDiscarded[i])
                    continue;
                KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
                vector<bool> vbInliers;
                int nInliers; bool bNoMore;

                Sim3Solver* pSolver = vpSim3Solvers[i];
                cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
                if(bNoMore) {
                    vbDiscarded[i]=true;
                    nCandidates--;
                }

                if (!Scm.empty()) {
                    vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast&t;MapPoint*>(NULL));
                    for(size_t j = 0, jend=vbInliers.size(); j < jend; j++) {
                        if(vbInliers[j])
                           vpMapPointMatches[j]=vvpMapPointMatches[i][j];
                    }

                    cv::Mat R = pSolver->GetEstimatedRotation();
                    cv::Mat t = pSolver->GetEstimatedTranslation();
                    const float s = pSolver->GetEstimatedScale();
                    matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

                    g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
                    const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

③ 当有一个候选帧有超过20个的内点时，停止遍历，并将之认定为闭环帧。同时还会根据当前帧到闭环帧的相似变换 Scm 以及闭环帧的世界位姿 Smw 计算当前帧的修正位姿 Scw， 修正位姿将分别以 g2o 和 opencv 两个库的数据结构保存到成员变量 mg2oScw 和 mScw 中。成员容器 mvpCurrentMatchedPoints 则记录了闭环帧与当前帧的匹配地图点。

                    if (nInliers >=20) {
                        bMatch = true; mpMatchedKF = pKF;
                        g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);

                        mg2oScw = gScm*gSmw;
                        mScw = Converter::toCvMat(mg2oScw);
                        mvpCurrentMatchedPoints = vpMapPointMatches;
                        break;
                    }
            }   }

最后还会对闭环帧的共视组中所有的地图点进行一次投影匹配。在下面的第67到77行中，遍历闭环帧及其邻接关键帧提取出其中的所有地图点。 78 行通过投影匹配确定在修正位姿下当前关键帧看到的地图点，记录到成员容器 mvpCurrentMatchedPoints 中。 剩下的代码就是统计一下匹配的地图点数量，只接受地图点大于 40 的情况。

            // 省略匹配失败的代码
            vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
            vpLoopConnectedKFs.push_back(mpMatchedKF);
            // 提取出闭环帧及其邻接关键帧的地图点
            mvpLoopMapPoints.clear();
            for(auto vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++) {
                KeyFrame* pKF = *vit;
                vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
                for(size_t i=0, iend=vpMapPoints.size(); i < iend; i++) {
                    MapPoint* pMP = vpMapPoints[i];
                    if(pMP && !pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId) {
                        mvpLoopMapPoints.push_back(pMP);
                        pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                    }
                }
            }
            matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

            int nTotalMatches = 0;
            for(size_t i=0; i < mvpCurrentMatchedPoints.size(); i++) {
                if(mvpCurrentMatchedPoints[i])
                    nTotalMatches++;
            }
            if (nTotalMatches >= 40) {
                for(int i=0; i < nInitialCandidates; i++)
                    if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                        mvpEnoughConsistentCandidates[i]->SetErase();
                return true;
            } else {
                for(int i=0; i < nInitialCandidates; i++)
                    mvpEnoughConsistentCandidates[i]->SetErase();
                mpCurrentKF->SetErase();
                return false;
        }   }

4. 完

本文中，我们先详细分析了 Horn 算法的原理， 结合代码，我们可以通过如下四步求解出 SE3 变换：

1. 通过样本坐标构建矩阵 \(N\)；
2. 对矩阵 \(n\) 特征值分解，找到最大特征值对应的特征向量，得到四元数\(\vec{q}\)，构建旋转矩阵 \(R\)；
3. 套用公式\((\ref{scale_factor})\)计算尺度因子 \(s\)
4. 将旋转矩阵 \(R\) 和尺度因子 \(s\) 代入公式\((\ref{translate_func})\)中求得平移向量\(\boldsymbol{r}_0\)

然后，简单过了一遍 LoopClosing 中根据相似变换建立的几何约束筛选候选关键帧的业务逻辑，大体上可以分为四个部分：

1. 为每一个候选帧构建一个 Sim3Solver 求解器；
2. 以 RANSAC 的方式初步求解各个候选帧的 Sim3，并通过投影匹配筛选内点；
3. 当有一个候选帧有超过20个的内点时，停止遍历，并将之认定为闭环帧；
4. 最后还会对闭环帧的共视组中所有的地图点进行一次投影匹配，得到不少于40个匹配地图点，才认定发生了一次闭环。