相似变换群与李代数

对于单目视觉SLAM，由于单目的不确定性，在闭环检测中为了解决尺度漂移，一般会用到相似变换群
Sim3,用来描述相似变换：
p ′ = [ s R t 0 1 ] p = s R p + t p'=\begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix}p=s\bm{Rp+t} p′=⎣⎡​sR0​t1​⎦⎤​p=sRp+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}
其指数映射为：
[ s R t 0 1 ] = exp ⁡ ( ζ ∧ ) = [ exp ⁡ ( σ ) + exp ⁡ ( ϕ ∧ ) J s ρ   0 T 1 ] \begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix}=\exp(\zeta ^\wedge)=\left[ {\begin{matrix} {\exp(\sigma)+\exp({\phi ^ \wedge }})& J_s\rho \\ ~\\ {{0^T}}&1 \end{matrix}} \right] ⎣⎡​sR0​t1​⎦⎤​=exp(ζ∧)=⎣⎡​exp(σ)+exp(ϕ∧) 0T​Js​ρ1​⎦⎤​
其中
J s = e σ − 1 σ I + σ e σ sin ⁡ θ + ( 1 − e σ cos ⁡ θ ) θ σ 2 + θ 2 a ∧ + ( e σ − 1 σ − ( e σ cos ⁡ θ − 1 ) σ + ( e σ sin ⁡ θ ) θ σ 2 + θ 2 ) a ∧ a ∧ J_s = \frac {e^{\sigma}-1}{\sigma}\bm I+\frac{\sigma e^\sigma \sin \theta+(1-e^\sigma\cos \theta)\theta} {\sigma^2+\theta^2}\bm a^{\wedge}+(\frac {e^{\sigma}-1} {\sigma}-\frac {(e^\sigma\cos \theta-1)\sigma+(e^\sigma \sin \theta)\theta} {\sigma^2+\theta^2})\bm a^{\wedge}\bm a^{\wedge} Js​=σeσ−1​I+σ2+θ2σeσsinθ+(1−eσcosθ)θ​a∧+(σeσ−1​−σ2+θ2(eσcosθ−1)σ+(eσsinθ)θ​)a∧a∧
 
 

回环检测-Sim3求解

参考文献[1]，求解。
对于Sim3群，有三对匹配点：
[ s R t 0 1 ] ， x i = s R x ˉ i + t i ∈ ( 1 , 2 , 3 ) \begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix}，x_i = s\bm{R} \bar x_i+t \quad i \in (1,2,3) ⎣⎡​sR0​t1​⎦⎤​，xi​=sRxˉi​+ti∈(1,2,3)
计算质心：
c = 1 3 ( x 1 + x 2 + x 3 ) , c ˉ = 1 3 ( x ˉ 1 + x ˉ 2 + x ˉ 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) c=31​(x1​+x2​+x3​),cˉ=31​(xˉ1​+xˉ2​+xˉ3​)
减去质心点：
y i = x i − c , y ˉ i = x ˉ i − c ˉ y_i = x_i-c,\quad \bar y_i =\bar x_i-\bar c yi​=xi​−c,yˉ​i​=xˉi​−cˉ
得到映射矩阵H：
H = y 1 y ˉ 1 T + y 2 y ˉ 2 T + y 3 y ˉ 3 T H = y_1 \bar y_1^T + y_2 \bar y_2^T+ y_3\bar y_3^T H=y1​yˉ​1T​+y2​yˉ​2T​+y3​yˉ​3T​
奇异值SVD分解：
H = U ⋅ W ⋅ V T H = U\cdot W\cdot V^T H=U⋅W⋅VT
求解得到R、t和尺度因子s：
R = V ⋅ U T , t = c − s R c ˉ   s = ( ∥ y 1 ∥ 2 2 + ∥ y 2 ∥ 2 2 + ∥ y 3 ∥ 2 2 ) 1 2 ( ∥ y ˉ 1 ∥ 2 2 + ∥ y ˉ 2 ∥ 2 2 + ∥ y ˉ 3 ∥ 2 2 ) 1 2 R = V\cdot U^T,\quad t=c-s\bm R\bar c\\~\\ s=\frac {(\left\|y_1 \right\|_2^2+\left\|y_2 \right\|_2^2+\left\|y_3 \right\|_2^2)^{\frac 1 2}} {(\left\| \bar y_1 \right\|_2^2+\left\|\bar y_2 \right\|_2^2+\left\|\bar y_3 \right\|_2^2)^{\frac 1 2}} R=V⋅UT,t=c−sRcˉ s=(∥yˉ​1​∥22​+∥yˉ​2​∥22​+∥yˉ​3​∥22​)21​(∥y1​∥22​+∥y2​∥22​+∥y3​∥22​)21​​
 
 

Sim3位姿图优化

我们知道SE3的位姿图优化：
e i j ( ξ i , ξ j ) = ln ⁡ [ exp ⁡ ( − ξ i j ∧ ) exp ⁡ ( − ξ i ∧ ) ⋅ exp ⁡ ( ξ j ∧ ) ] ∨ ,   e i j ( ξ i , ξ j ) = ln ⁡ [ T i j − 1 ⋅ T i − 1 ⋅ T j ] ∨ e_{ij}(\xi_i,\xi_j)=\ln[\exp(-\xi_{ij} ^\land) \exp(-\xi_i ^\land)\cdot \exp(\xi_j ^\land)]^\vee, \\ ~\\e_{ij}(\xi_i,\xi_j)=\ln[T_{ij}^{-1}\cdot T_i^{-1}\cdot T_j]^\vee eij​(ξi​,ξj​)=ln[exp(−ξij∧​)exp(−ξi∧​)⋅exp(ξj∧​)]∨, eij​(ξi​,ξj​)=ln[Tij−1​⋅Ti−1​⋅Tj​]∨
同理，对于Sim3的位姿图优化的误差函数可以表示为：
e i j ( ζ i , ζ j ) = ln ⁡ [ exp ⁡ ( − ζ i j ∧ ) exp ⁡ ( − ζ i ∧ ) ⋅ exp ⁡ ( ζ j ∧ ) ] ∨   即 e i j ( ζ i , ζ j ) = ln ⁡ [ S i j − 1 ⋅ S i − 1 ⋅ S j ] ∨ e_{ij}(\zeta_i,\zeta_j)=\ln[\exp(-\zeta_{ij} ^\land) \exp(-\zeta_i ^\land)\cdot \exp(\zeta_j ^\land)]^\vee\\ ~\\ 即\quad e_{ij}(\zeta_i,\zeta_j)=\ln[S_{ij}^{-1}\cdot S_i^{-1}\cdot S_j]^\vee eij​(ζi​,ζj​)=ln[exp(−ζij∧​)exp(−ζi∧​)⋅exp(ζj∧​)]∨ 即eij​(ζi​,ζj​)=ln[Sij−1​⋅Si−1​⋅Sj​]∨
为了为优化作准备，我们保持每个位姿旋转和平移不变，并设置s=1，只有当回环检测的时候 s ≠ 1 s\ne 1 s​=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 源码