SLAM--单目尺度漂移(相似变换群Sim3)

相似变换群与李代数

对于单目视觉SLAM，由于单目的不确定性，在闭环检测中为了解决尺度漂移，一般会用到相似变换群
Sim3,用来描述相似变换：
$$p^{\prime }=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}\right]p=s\mathbf{R}\mathbf{p}+\mathbf{t}$$
和SO3、SE3类似，可以将其描述为群：
S i m ( 3 ) = { S = [ s R t 0 1 ] ∈ R 4 × 4 } Sim(3)=\{S= \begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix} \in {\mathbb{R}^{4\times 4}} \} Sim(3)={S=⎣⎡​sR0​t1​⎦⎤​∈R4×4}
对应的李代数sim(3)是一个7维的向量（7个自由度）：
s i m ( 3 ) = { ζ = [ ρ ϕ σ ] ∈ R 7 ∣ ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ζ ∧ = [ σ I + ϕ ∧ ρ   0 T 0 ] ∈ R 4 × 4 } sim(3) = \{ \zeta = \left[ {\begin{matrix} \rho \\ \phi \\ \sigma \end{matrix}} \right] \in {\mathbb{R}^7}|\rho \in {\mathbb{R}^3},\phi \in so(3),{\zeta ^ \wedge } = \left[ {\begin{matrix} {\sigma I+{\phi ^ \wedge }}&\rho \\ ~\\ {{0^T}}&0 \end{matrix}} \right]\in {\mathbb{R}^{4\times 4}} \} sim(3)={ζ=⎣⎡​ρϕσ​⎦⎤​∈R7∣ρ∈R3,ϕ∈so(3),ζ∧=⎣⎡​σI+ϕ∧ 0T​ρ0​⎦⎤​∈R4×4}
其指数映射为：
$$\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}\right]=\exp ({\zeta }^{\wedge })=\left[\begin{array}{cc}\exp (\sigma )+\exp ({\varphi }^{\wedge })& J_s\rho \\ & \\ 0^T& 1\end{array}\right]$$
其中
$$J_s=\frac{e^{\sigma }-1}{\sigma }\mathbf{I}+\frac{\sigma e^{\sigma }\sin \theta +(1-e^{\sigma }\cos \theta )\theta }{{\sigma }^2+{\theta }^2}{\mathbf{a}}^{\wedge }+(\frac{e^{\sigma }-1}{\sigma }-\frac{(e^{\sigma }\cos \theta -1)\sigma +(e^{\sigma }\sin \theta )\theta }{{\sigma }^2+{\theta }^2}){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }$$
 
 

回环检测-Sim3求解

参考文献[1]，求解。
对于Sim3群，有三对匹配点：
$$\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}\right]，x_i=s\mathbf{R}{\bar{x}}_i+ti\in (1,2,3)$$
计算质心：
$$c=\frac{1}{3}(x_1+x_2+x_3),\bar{c}=\frac{1}{3}({\bar{x}}_1+{\bar{x}}_2+{\bar{x}}_3)$$
减去质心点：
$$y_i=x_i-c,{\bar{y}}_i={\bar{x}}_i-\bar{c}$$
得到映射矩阵H：
$$H=y_1{\bar{y}}_1^T+y_2{\bar{y}}_2^T+y_3{\bar{y}}_3^T$$
奇异值SVD分解：
$$H=U\cdot W\cdot V^T$$
求解得到R、t和尺度因子s：
$$\begin{gathered} R=V\cdot U^T,t=c-s\mathbf{R}\bar{c} \\ s=\frac{({\Vert y_1\Vert }_2^2+{\Vert y_2\Vert }_2^2+{\Vert y_3\Vert }_2^2{)}^{\frac{1}{2}}}{({\Vert {\bar{y}}_1\Vert }_2^2+{\Vert {\bar{y}}_2\Vert }_2^2+{\Vert {\bar{y}}_3\Vert }_2^2{)}^{\frac{1}{2}}} \end{gathered}$$
 
 

Sim3位姿图优化

我们知道SE3的位姿图优化：
$$\begin{gathered} e_{ij}({\xi }_i,{\xi }_j)=\ln [\exp (-{\xi }_{ij}^{\wedge })\exp (-{\xi }_i^{\wedge })\cdot \exp ({\xi }_j^{\wedge }){]}^{\vee }, \\ {e}_{ij}({\xi }_i,{\xi }_j)=\ln [T_{ij}^{-1}\cdot T_i^{-1}\cdot T_j{]}^{\vee } \end{gathered}$$
同理，对于Sim3的位姿图优化的误差函数可以表示为：
$$\begin{gathered} e_{ij}({\zeta }_i,{\zeta }_j)=\ln [\exp (-{\zeta }_{ij}^{\wedge })\exp (-{\zeta }_i^{\wedge })\cdot \exp ({\zeta }_j^{\wedge }){]}^{\vee } \\ 即e_{ij}({\zeta }_i,{\zeta }_j)=\ln [S_{ij}^{-1}\cdot S_i^{-1}\cdot S_j{]}^{\vee } \end{gathered}$$
为了为优化作准备，我们保持每个位姿旋转和平移不变，并设置s=1，只有当回环检测的时候$s\ne 1$；

雅克比矩阵推导略；具体参考文献[2].

我们依然可以用g2o进行sim3的优化.

附上ORB-SLAM2中优化Sim3的源码：

int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolverX::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();

    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // Calibration
    const cv::Mat &K1 = pKF1->mK;
    const cv::Mat &K2 = pKF2->mK;

    // Camera poses
    const cv::Mat R1w = pKF1->GetRotation();
    const cv::Mat t1w = pKF1->GetTranslation();
    const cv::Mat R2w = pKF2->GetRotation();
    const cv::Mat t2w = pKF2->GetTranslation();

    // Set Sim3 vertex
    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();    
    vSim3->_fix_scale=bFixScale;
    vSim3->setEstimate(g2oS12);
    vSim3->setId(0);
    vSim3->setFixed(false);
    vSim3->_principle_point1[0] = K1.at<float>(0,2);
    vSim3->_principle_point1[1] = K1.at<float>(1,2);
    vSim3->_focal_length1[0] = K1.at<float>(0,0);
    vSim3->_focal_length1[1] = K1.at<float>(1,1);
    vSim3->_principle_point2[0] = K2.at<float>(0,2);
    vSim3->_principle_point2[1] = K2.at<float>(1,2);
    vSim3->_focal_length2[0] = K2.at<float>(0,0);
    vSim3->_focal_length2[1] = K2.at<float>(1,1);
    optimizer.addVertex(vSim3);

    // Set MapPoint vertices
    const int N = vpMatches1.size();
    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
    vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
    vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;
    vector<size_t> vnIndexEdge;

    vnIndexEdge.reserve(2*N);
    vpEdges12.reserve(2*N);
    vpEdges21.reserve(2*N);

    const float deltaHuber = sqrt(th2);

    int nCorrespondences = 0;

    for(int i=0; i<N; i++)
    {
        if(!vpMatches1[i])
            continue;

        MapPoint* pMP1 = vpMapPoints1[i];
        MapPoint* pMP2 = vpMatches1[i];

        const int id1 = 2*i+1;
        const int id2 = 2*(i+1);

        const int i2 = pMP2->GetIndexInKeyFrame(pKF2);

        if(pMP1 && pMP2)
        {
            if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
            {
                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D1w = pMP1->GetWorldPos();
                cv::Mat P3D1c = R1w*P3D1w + t1w;
                vPoint1->setEstimate(Converter::toVector3d(P3D1c));
                vPoint1->setId(id1);
                vPoint1->setFixed(true);
                optimizer.addVertex(vPoint1);

                g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D2w = pMP2->GetWorldPos();
                cv::Mat P3D2c = R2w*P3D2w + t2w;
                vPoint2->setEstimate(Converter::toVector3d(P3D2c));
                vPoint2->setId(id2);
                vPoint2->setFixed(true);
                optimizer.addVertex(vPoint2);
            }
            else
                continue;
        }
        else
            continue;

        nCorrespondences++;

        // Set edge x1 = S12*X2
        Eigen::Matrix<double,2,1> obs1;
        const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
        obs1 << kpUn1.pt.x, kpUn1.pt.y;

        g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
        e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
        e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        e12->setMeasurement(obs1);
        const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
        e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);

        g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
        e12->setRobustKernel(rk1);
        rk1->setDelta(deltaHuber);
        optimizer.addEdge(e12);

        // Set edge x2 = S21*X1
        Eigen::Matrix<double,2,1> obs2;
        const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
        obs2 << kpUn2.pt.x, kpUn2.pt.y;

        g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();

        e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
        e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        e21->setMeasurement(obs2);
        float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
        e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);

        g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
        e21->setRobustKernel(rk2);
        rk2->setDelta(deltaHuber);
        optimizer.addEdge(e21);

        vpEdges12.push_back(e12);
        vpEdges21.push_back(e21);
        vnIndexEdge.push_back(i);
    }

    // Optimize!
    optimizer.initializeOptimization();
    optimizer.optimize(5);

    // Check inliers
    int nBad=0;
    for(size_t i=0; i<vpEdges12.size();i++)
    {
        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
        if(!e12 || !e21)
            continue;

        if(e12->chi2()>th2 || e21->chi2()>th2)
        {
            size_t idx = vnIndexEdge[i];
            vpMatches1[idx]=static_cast<MapPoint*>(NULL);
            optimizer.removeEdge(e12);
            optimizer.removeEdge(e21);
            vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
            vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
            nBad++;
        }
    }

    int nMoreIterations;
    if(nBad>0)
        nMoreIterations=10;
    else
        nMoreIterations=5;

    if(nCorrespondences-nBad<10)
        return 0;

    // Optimize again only with inliers

    optimizer.initializeOptimization();
    optimizer.optimize(nMoreIterations);

    int nIn = 0;
    for(size_t i=0; i<vpEdges12.size();i++)
    {
        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
        if(!e12 || !e21)
            continue;

        if(e12->chi2()>th2 || e21->chi2()>th2)
        {
            size_t idx = vnIndexEdge[i];
            vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        }
        else
            nIn++;
    }

    // Recover optimized Sim3
    g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
    g2oS12= vSim3_recov->estimate();

    return nIn;
}

参考文献

1.Horn, Berthold, K, et al. Closed-form solution of absolute orientation using unit quaternions[J]. Journal of the Optical Society of America A, 1987.
2.Strasdat H . Local Accuracy and Global Consistency for Efficient SLAM[D]. PhD thesis, Imperial College London, 2012.
3.视觉SLAM十四讲–高翔
4.ORB-SLAM2 源码