ORB-SLAM2代码阅读笔记（十）：sim3求解

最新推荐文章于 2024-04-21 09:47:38 发布

文科升

最新推荐文章于 2024-04-21 09:47:38 发布

阅读量1.3w

点赞数 41

分类专栏： SLAM 文章标签： SLAM SIM3 ORB-SLAM2

本文链接：https://blog.csdn.net/moyu123456789/article/details/91947539

版权

SLAM 专栏收录该内容

35 篇文章 88 订阅

订阅专栏

Table of Contents

1）sim3求解器构造函数Sim3Solver

2）sim3迭代求解Sim3Solver::iterate

本篇笔记对 ORB-SLAM2代码阅读笔记（八）：LoopClosing线程中提到的sim3进行了分析，以熟悉使用三对匹配点求解位姿变换的方法。

1.sim3的简单概述

sim3简单来说，就是使用3对匹配点来进行相似变换（similarity transformation）的求解，进而解出两个坐标系之间的旋转矩阵、平移向量和尺度。ORB-SLAM2中使用的sim3求解方法来自 Horn 1987, Closed-form solution of absolute orientation using unit quaternions 这篇论文。所以，需要了解一下这篇论文中提出的求解思想，以帮助理解代码。

ORB-SLAM2系统在LoopClosing线程中，当检测到闭环候选帧的时候，就需要对当前关键帧和对应的闭环候选帧之间计算其变换关系。这时需要用当前关键帧和其对应的闭环候选帧进行sim3求解，这里的sim3求解是对当前关键帧和闭环候选帧之间匹配的MapPoint进行sim3求解。通过sim3变换解出当前关键帧和闭环候选帧的匹配MapPoint之间的旋转矩阵R、平移向量t、尺度变换s，也就得到了当前关键帧到闭环关键帧之间的sim3变换gScm。使用这个sim3变换gScm乘上闭环关键帧的sim3位姿gSmw，mg2oScw=gScm*gSmw的乘积mg2oScw就是当前关键帧的sim3位姿，之后在闭环校正中就可以使用这个sim3位姿转换为SE3位姿后对当前关键帧进行位姿校正（当然也要对关键帧对应的MapPoints以及其共视的关键帧进行校正）。当然在实际代码中，这个过程还要做多次投影和优化操作以确保更高的准确率和精度，这个可以阅读ORB-SLAM2该部分的代码来体会。

以下图为例，A为当前关键帧，B为候选关键帧。

当相机从B处开始运动到A处的时候，检测到B为A的闭环候选帧。此时，考虑到相机从B运动到A的过程中不光会产生旋转和平移的误差，同时也会产生尺度漂移的累积，需要计算A和B之间的sim3变换，来找到A和B之间的sim3变换（包括旋转矩阵R、平移向量t、尺度变换s）,有了这些值之后，就可以对关键帧A的位姿进行纠正。

2.sim3算法介绍

Closed-form solution of absolute orientation using unit quaternions论文提供了一种算法，通过两个坐标系之间3对匹配点来确定两个坐标系之间的变换关系。

1)3对匹配的3D点建立坐标系

如上图所示，在左右两个坐标系中各有3个点，左侧三个点分别是 $$r_l,_1$ ， $$r_l,_2$ ， $$r_l,_3$ ，右侧三个点为 $r_r,_1$ ， $r_r,_2$ ， $r_{r,3}$ 。

可以看出，左侧的 $r_{l,1}$ 和右侧的 $r_{r,1}$ 都处于坐标系的原点处，则我们可以计算出在X轴、Y轴和Z轴上的单位向量如下：

X轴：x轴上的向量为 $x_l=r_{l,2}-r_{l,1}$$ ，对 $x_l$ 除以其模长得到在x轴方向上的单位向量 $\hat{x_l}=x_l/\left \| x_l \right \|$

图中这种情况下 $x_{l,1}$ 为原点， $r_{l,2}$ 在x轴方向上，所以两个点相减后所得向量在x轴上，对向量除以模长就是x轴上的单位向量。

Y轴：y轴上的向量为 $y_l=(r_{l,3}-r_{l,1})-\left [ \left ( r_{l,3}-r_{l,1} \right )\cdot \hat{x_l} \right ]\cdot \hat{x_l}$ ，对 $y_l$ 除以 $y_l$ 的模长得到y轴方向上的单位向量 $\hat{y_l}=\hat{y_l}/\left \| y_l \right \|$

$r_{l,3}-r_{l,1}$ 所得为从 $r_{l,1}$ 到 $r_{l,3}$ 这两个点之间的向量， $\left ( r_{l,3}-r_{l,1} \right )\cdot\hat{x_l}$ 表示向量在x轴方向上的单位长度， $\left [ \left ( r_{l,3}-r_{l,1} \right )\cdot \hat{x_l} \right ]\cdot \hat{x_l}$ 表示该向量在x轴方向上的向量的长度。则向量减去其在x轴方向的向量就得到了在y轴方向上的向量。

Z轴：由于z轴垂直与x轴和y轴，所以z轴所在方向也就是x和y轴方向向量叉乘乘积的方向（两个向量叉乘后的乘积所在的向量方向和这两个向量垂直），则z轴方向上的单位向量为 $\hat{z_l}=\hat{x_l}\times \hat{y_l}$

同理，也可推出右侧坐标系各个轴上的单位向量 $\hat{x_r},\hat{y_r},\hat{z_r}$ 。

2)旋转矩阵计算

根据1）中所得的坐标系单位向量，可以得到左右两侧坐标系的各个方向单位向量构成的矩阵： $\large M_l=\left | \hat{x_l} \hat{y_l}\hat{z_l}\right |$ ， $\large M_l=\left | \hat{x_r}\hat{y_r} \hat{z_r} \right |$

此时，如果左侧坐标系中有一个向量 $r_l$ ，则 $M_l^T\cdot r_l$ 可以计算结果为 $r_l$ 在三个坐标轴方向上的向量值。 $M_r$ 左乘 $M_l^T\cdot r_l$ 后所得结果为变换到右侧坐标系中的向量 $\large r_r=M_rM_l^Tr_l$ 。

则可以推导出从左侧坐标系旋转到右侧坐标系中的旋转矩阵为： $\large {\color{Red} R=M_r{M_l^T}}$

3）平移向量计算

假设左右坐标系中各有 $n$ 各点，他们在左右两侧坐标系中的测量坐标值为 $\left \{ r_{l,i} \right \}$ 和 $\left \{ r_{r,i} \right \}$ ， $i$ 的取值从1到 $n$ 。

此时，一个向量从左侧坐标系到右侧坐标系的变换可表示为： $\large r_r = sR(r_l)+r_0$ 其中，s为尺度变换， $\large r_0$ 为平移偏移量。 $\large R(r_l)$ 表示 $\large r_l$ 的旋转。

实际当中，两个坐标系之间的变换不会那么容易计算出精确的变换向量，和机器学习中采用方法相同，都是采用优化的方法（最小化误差的方法）来求解的，也就是使用最小二乘法来求解。此时，容易看出这里的误差为：

$\large e_i=r_{r,i}-sR\left ( r_{l,i} \right )-r_0.$

那么，求解的最小二乘问题变成了求解：

$\large min\sum_{i=1}^n\left \| e_i \right \|^2.$

那么，怎么来求解这个最小误差呢？我们先把要求解的表达式放在这里。这里我们要求解的是平移向量，也就是上边的 $\large r_0$ 。看看作者是怎么引入问题求解方法的。

计算左侧和右侧坐标系中所有点的质心（其实也就是所有点的中心）：

$\large \bar{r_l}=\frac{1}{n}\sum _{i=1}^nr_{l,i}$ ， $\large \bar{r_r}=\frac{1}{n}\sum _{i=1}^nr_{r,i}$

则每一个点距离质心的距离为：

$\large {r_{l,i}}'=r_{l,i}-\bar{r_l}$ $\large {r_{r,i}}'=r_{r,i}-\bar{r_r}$

则，我们也可以知道： $\large \sum _{i=1}^{n}r_{l,i}^{'}=0$ $\large \sum _{i=1}^{n}r_{r,i}^{'}=0$

根据上边得出的变换公式可得：

$\large r_{r,i}^{'}=sR(r_{l,i}^{'})+{r_0}'$ $\large \Rightarrow$ $\large {r_0}'={r_{r,i}}'-sR({r_{l,i}}')$

$\large \Rightarrow$ $\large {r_0}'=r_{r,i}-\bar{r_r}-sR(r_{l,i}-\bar{r_l})$

$\large \Rightarrow$ $\large {r_0}'=r_{r,i}-\bar{r_r}-sR(r_{l,i})+sR(\bar{r_l})$

$\large \Rightarrow$ $\large {r_0}'=r_0-\bar{r_r}+sR(\bar{r_l})$

此时，优化函数如下：

$\large \sum _{i=1}^{n}\left \| e_i \right \|^2=\sum _{i=1}^n\left \| {r_{r,i}}'-\left [ sR({r_{l,i}}')+{r_0}' \right ] \right \|^2$

$\large =\sum _{i=1}^{n}\left \| {r_{r,i}}'-sR({r_{l,i}}') \right \|^2-2{r_0}'\cdot \sum _{i=1}^{n}\left [ {r_{r,i}}'-sR({r_{l,i}}') \right ]+n\left \| {r_0}' \right \|^2$

对上边的式子，容易计算出中间部分 $\large 2{r_0}'\cdot \sum _{i=1}^{n}\left [ {r_{r,i}}'-sR({r_{l,i}}') \right ]=0$

此时剩下第一部分和第三部分，第一部分和 $\large {r_0}'$ 没关系，第三部分不可能为负值。所以，优化函数在 $\large {r_0}'=0$ 的情况下取得最小值。

带入上式： $\large {r_0}'=r_0-\bar{r_r}+sR(\bar{r_l})$ 中，可得平移向量 $\large t$ :

$\large {\color{Red} t=r_0=\bar{r_r}-sR(\bar{r_l})}$

4）尺度计算

由于 $\large {r_0}'=0$ ，所以可得误差函数：

$\large \sum _{i=1}^{n}\left \| e_i \right \|^2=\sum _{i=1}^n\left \| {r_{r,i}}'-\left [ sR({r_{l,i}}')+{r_0}' \right ] \right \|^2$

$\large =\sum _{i=1}^n\left \| {r_{r,i}}'-\left [ sR({r_{l,i}}') \right ] \right \|^2$

$\large =\sum _{i=1}^n\left \| {r_{r,i}}' \right \|^2-2s\sum _{i=1}^n{r_{r,i}}'\cdot R({r_{l,i}}')+s^2\sum _{i=1}^{n}\left \| R({r_{l,i}}') \right \|^2$

我们进一步可以把这个形式写成如下形式：

令： $\large \sum _{i=1}^{n}\left \| e_i \right \|^2=S_r-2sD+s^2S_l$

$\large =(s\sqrt{S_l}-D/\sqrt{S_l})^2+(S_rS_l-D^{2})/S_l$

此时，要是误差取最小值，则必须第一项为0，此时

$\large {\color{Red} {\color{Red} }s=D/S_l=\left ( \sum _{i=1}^n{r_{r,i}}'\cdot R({r_{l,i}}') \right )/\sum _{i=1}^{n}\left \| R({r_{l,i}}') \right \|^2}$

根据对称性可得： $\large r_r=sR(r_l)+r_0$ $\large r_l=\bar{s}\bar{R}(r_r)+\bar{r_0}$

我们期望计算出来的值为： $\large \bar{s}=1/s$

$\large \bar{r_0}=-\frac{1}{s}R^{-1}(r_0)$

$\large \bar{R}=R^{-1}$

但是，此时 $\large \bar{s}=1/s\neq\left ( \sum _{i=1}^n{r_{r,i}}'\cdot R({r_{l,i}}') \right )/\sum _{i=1}^{n}\left \| R({r_{l,i}}') \right \|^2}$

当已知两个系统中的一个系统的坐标比另一个系统的坐标精度高得多时，上述两个不对称结果中的一个可能是合适的。

如果两组测量中的误差相似，则对误差项使用对称表达式更为合理：

$\large e_i=\frac{1}{\sqrt{S}}{r_{r,i}}'-\sqrt{s}R({r_{l,i}}')$

则上边式子变为： $\large \sum _{i=1}^{n}\left \| e_i \right \|^2=\frac{1}{s}S_r-2D+sS_r=(\sqrt{s}S_l-\frac{1}{\sqrt{s}}S_r)^2+2(S_lS_r-D)$

该式求最小值，可计算得: $\large {\color{Red} s=(\sum _{i=1}^n\left \| {r_{r,i}}' \right \|^2/\sum _{i=1}^n\left \| {r_{l,i}}' \right \|^2)^\frac{1}{2}}$

这种对称结果的一个有点是，它允许人们在不需要知道旋转的情况下确定尺度。重要的是，旋转的确定不受我们选择比例因子的三个值之一的影响。在每种情况下，当D尽可能大时，误差能取得最小值。也就是说，我们必须选择使 $\large {\color{Red} \sum _{i=1}^n{r_{r,i}}'\cdot R\left ( {r_{l,i}}' \right )}$ 尽可能大的旋转值。

注意：这里的意思是说，只有在 $\large D={\color{Red} \sum _{i=1}^n{r_{r,i}}'\cdot R\left ( {r_{l,i}}' \right )}$ 取得最大值的情况下，误差才会最小，所以为了求得最小误差，我们应该去求最大的 $\large D$ .

现在，我们把问题变成了求最大的 $\large D$ 。

旋转可以使用多种方式进行表达，此处，引入了四元数来表达。

一个四元数表达如下： $\large \qq$ $\large \overset{\circ }{q}= q_0+iq_x+jq_y+kq_z$

其中 $\large q_0$ 为实部，并且满足： $\large i^2=-1,$ $\large j^2=-1,$ $\large k^2=-1;$

$\large ij=k,$ $\large jk=i,$ $\large ki=j;$

$\large ji=-k,$ $\large kj=-i,$ $\large ik=-j;$

此时两个四元数的点乘积为： $\large \overset{\circ}{p}\cdot \overset{\circ}{q}=p_{0}q_{0}+p_{x}q_{x}+p_yq_y+p_zq_z$

定义 $\large \overset{\circ}{q}$ 的共轭向量为： $\large \overset{\circ}{q}^\ast =q_0-iq_x-jq_y-kq_z$

则有： $\large \overset{\circ}{q}\cdot \overset{\circ}{q}^\ast =q_0^2+q_x^2+q_y^2+q_z^2=\overset{\circ}{q}\cdot \overset{\circ}{q}$

此时， $\large D={\color{Red} \sum _{i=1}^n{r_{r,i}}'\cdot R\left ( {r_{l,i}}' \right )}$

可以用单位四元数表达为如下形式：

$\large \sum _{i=1}^n\left ( \overset{\circ}{q} \overset{\circ}{{r}'}_{l,i} \overset{\circ}{q}^*\right )\cdot \overset{\circ}{{r}'_{r,i}}.$

对该式可改写为： $\large \sum _{i=1}^n\left ( \overset{\circ}{q} \overset{\circ}{{r}'}_{l,i} \right )\cdot \left ( \overset{\circ}{{r}'_{r,i}} \overset{\circ}{q}\right ).$

此时，假设 $\large {r_{l,i}}'=\left ( {x_{l,i}}',{y_{l,i}}',{z_{l,i}}' \right )^T$ $\large {r_{r,i}}'=\left ( {x_{r,i}}',{y_{r,i}}',{z_{r,i}}' \right )^T$

则 $\large M=\sum _{i=1}^n{r_{l,i}}'{r_{r,i}}'^T$ ，可将 $\large M$ 定义为以下形式

$\large M=\begin{bmatrix} S_{xx} & S_{xy} & S_{xz}\\ S_{yx} & S_{yy} & S_{yz}\\ S_{zx} & S_{zy }& S_{zz} \end{bmatrix}$

定义了M就是为了用其中的元素来表示N,N定义如下：

$\large N=\begin{bmatrix} (S_{xx}+S_{yy}+S_{zz}) &S_{yz}-S_{zy} &S_{zx}-S_{xz} &S_{xy}-S_{yx} \\ S_{yz}-S_{zy}& (S_{xx}-S_{yy}-S_{zz}) &S_{xy}+S_{yx} & S_{zx}+S_{xz}\\ S_{zx}-S_{xz}& S_{xy}+S_{yx} & (-S_{xx}+S_{yy}-S_{zz}) &S_{yz}+S_{zy}\\ S_{xy}-S_{yx}& S_{zx}+S_{xz}&S_{yz}+S_{zy} & (-S_{xx}-S_{yy}+S_{zz}) \end{bmatrix}$

此时，对N矩阵进行特征值分解求解最大特征值，该特征值对应的特征向量就是待求的四元数。

四元数转欧拉角: $\large {\color{Red} q=cos\frac{\Theta }{2}+nsin\frac{\Theta }{2}}$

根据四元数可以求出旋转矩阵R、平移向量t和尺度s。

3.代码解析

在Closed-form solution of absolute orientation using unit quaternions这篇论文的结论部分，作者坦言该论文中的算法表达相对复杂一些，但是好处是一些程序库中已经实现了该算法的sim3求解，所以对我们使用者来说，只要调用库函数就行了，这样就简单了好多。

ORB-SLAM2中sim3求解流程相关代码位于LoopClosing::ComputeSim3()中。主要的求解函数如下：

1）sim3求解器构造函数Sim3Solver

Sim3Solver函数用于针对每一个和当前关键帧存在闭环关系的闭环候选关键帧之间构造sim3求解器。求解器中，主要是确定两个关键帧中匹配的MapPoint的对应关系，构造好匹配的MapPoint点对列表，并设置Ransac相关参数用于后续iterate中求解使用。

Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale):
    mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale)
{
    mpKF1 = pKF1;
    mpKF2 = pKF2;

    vector<MapPoint*> vpKeyFrameMP1 = pKF1->GetMapPointMatches();

    mN1 = vpMatched12.size();

    mvpMapPoints1.reserve(mN1);
    mvpMapPoints2.reserve(mN1);
    mvpMatches12 = vpMatched12;
    mvnIndices1.reserve(mN1);
    mvX3Dc1.reserve(mN1);
    mvX3Dc2.reserve(mN1);

    cv::Mat Rcw1 = pKF1->GetRotation();
    cv::Mat tcw1 = pKF1->GetTranslation();
    cv::Mat Rcw2 = pKF2->GetRotation();
    cv::Mat tcw2 = pKF2->GetTranslation();

    mvAllIndices.reserve(mN1);

    size_t idx=0;
    for(int i1=0; i1<mN1; i1++)
    {
        if(vpMatched12[i1])
        {
            MapPoint* pMP1 = vpKeyFrameMP1[i1];
            MapPoint* pMP2 = vpMatched12[i1];

            if(!pMP1)
                continue;

            if(pMP1->isBad() || pMP2->isBad())
                continue;
            //获取MapPoint中记录的能看到该MapPoint的keyframe中对应的关键点的index
            int indexKF1 = pMP1->GetIndexInKeyFrame(pKF1);
            int indexKF2 = pMP2->GetIndexInKeyFrame(pKF2);

            if(indexKF1<0 || indexKF2<0)
                continue;

            const cv::KeyPoint &kp1 = pKF1->mvKeysUn[indexKF1];
            const cv::KeyPoint &kp2 = pKF2->mvKeysUn[indexKF2];

            const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave];
            const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];

            mvnMaxError1.push_back(9.210*sigmaSquare1);
            mvnMaxError2.push_back(9.210*sigmaSquare2);

            mvpMapPoints1.push_back(pMP1);
            mvpMapPoints2.push_back(pMP2);
            mvnIndices1.push_back(i1);
            //pMP1对应的相机坐标
            cv::Mat X3D1w = pMP1->GetWorldPos();
            mvX3Dc1.push_back(Rcw1*X3D1w+tcw1);

            cv::Mat X3D2w = pMP2->GetWorldPos();
            //pMP2对应的相机坐标
            mvX3Dc2.push_back(Rcw2*X3D2w+tcw2);

            mvAllIndices.push_back(idx);
            idx++;
        }
    }
    //相机内参
    mK1 = pKF1->mK;
    mK2 = pKF2->mK;
    //FromCameraToImage函数计算从相机坐标到像素坐标的投影点，mvP1im1为投影点列表。
    FromCameraToImage(mvX3Dc1,mvP1im1,mK1);
    FromCameraToImage(mvX3Dc2,mvP2im2,mK2);

    SetRansacParameters();
}

其中调用的函数FromCameraToImage代码如下：

/**
 * 计算从相机坐标到像素坐标的投影
*/
void Sim3Solver::FromCameraToImage(const vector<cv::Mat> &vP3Dc, vector<cv::Mat> &vP2D, cv::Mat K)
{
    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);

    vP2D.clear();
    //vP3Dc为相机坐标，vP2D为要投影到的像素点坐标
    vP2D.reserve(vP3Dc.size());

    for(size_t i=0, iend=vP3Dc.size(); i<iend; i++)
    {
        const float invz = 1/(vP3Dc[i].at<float>(2));
        const float x = vP3Dc[i].at<float>(0)*invz;
        const float y = vP3Dc[i].at<float>(1)*invz;

        vP2D.push_back((cv::Mat_<float>(2,1) << fx*x+cx, fy*y+cy));
    }
}

2）sim3迭代求解Sim3Solver::iterate

iterate函数是开始进行sim3求解的函数。遍历1）中构造好的每个求解器，调用求解器中的iterate函数进行求解。ORB-SLAM2中迭代5次进行求解。

cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
{
    bNoMore = false;
    vbInliers = vector<bool>(mN1,false);
    nInliers=0;

    if(N<mRansacMinInliers)
    {
        bNoMore = true;
        return cv::Mat();
    }

    vector<size_t> vAvailableIndices;

    cv::Mat P3Dc1i(3,3,CV_32F);
    cv::Mat P3Dc2i(3,3,CV_32F);

    int nCurrentIterations = 0;
    while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations)
    {
        nCurrentIterations++;
        mnIterations++;

        vAvailableIndices = mvAllIndices;

        // Get min set of points
        // 每次随机获取3对匹配点
        for(short i = 0; i < 3; ++i)
        {
            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);

            int idx = vAvailableIndices[randi];

            mvX3Dc1[idx].copyTo(P3Dc1i.col(i));
            mvX3Dc2[idx].copyTo(P3Dc2i.col(i));

            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
        //进行sim3求解
        ComputeSim3(P3Dc1i,P3Dc2i);

        CheckInliers();

        if(mnInliersi>=mnBestInliers)
        {
            mvbBestInliers = mvbInliersi;
            mnBestInliers = mnInliersi;
            mBestT12 = mT12i.clone();
            mBestRotation = mR12i.clone();
            mBestTranslation = mt12i.clone();
            mBestScale = ms12i;

            if(mnInliersi>mRansacMinInliers)
            {
                nInliers = mnInliersi;
                for(int i=0; i<N; i++)
                    if(mvbInliersi[i])
                        vbInliers[mvnIndices1[i]] = true;
                return mBestT12;
            }
        }
    }

    if(mnIterations>=mRansacMaxIts)
        bNoMore=true;

    return cv::Mat();
}

/**
 * 计算sim3
*/
void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
{
    // Custom implementation of:
    // Horn 1987, Closed-form solution of absolute orientataion using unit quaternions

    // Step 1: Centroid and relative coordinates

    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);

    // Step 2: Compute M matrix

    cv::Mat M = Pr2*Pr1.t();

    // Step 3: Compute N matrix

    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);
    N12 = M.at<float>(1,2)-M.at<float>(2,1);
    N13 = M.at<float>(2,0)-M.at<float>(0,2);
    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);


    // Step 4: Eigenvector of the highest eigenvalue

    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)

    // Rotation angle. sin is the norm of the imaginary part, cos is the real part
    double ang=atan2(norm(vec),evec.at<float>(0,0));

    vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half

    mR12i.create(3,3,P1.type());

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

    // Step 5: Rotate set 2

    cv::Mat P3 = mR12i*Pr2;

    // Step 6: Scale

    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;

    // Step 7: Translation

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

    // Step 8: Transformation

    // Step 8.1 T12
    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));

    // Step 8.2 T21

    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));
}

文科升

关注

41
点赞
踩
154

收藏

觉得还不错? 一键收藏
8
评论
ORB-SLAM2代码阅读笔记（十）：sim3求解

Table of Contents1.sim3的简单概述2.sim3算法介绍1)3对匹配的3D点建立坐标系2)旋转矩阵计算3）平移向量计算4）尺度计算3.代码解析1）sim3求解器构造函数Sim3Solver2）sim3迭代求解Sim3Solver::iterate本篇笔记对 ORB-SLAM2代码阅读笔记（八）：LoopClosing线程中提到的s...
复制链接

扫一扫