【SLAM学习笔记】10-ORB_SLAM3关键源码分析⑧ Optimizer（五）sim3优化

2021SC@SDUSC

1.前言

这一部分代码量巨大，查阅了很多资料结合来看的代码，将分为以下部分进行分析

1. 单帧优化
2. 局部地图优化
3. 全局优化
4. 尺度与重力优化
5. sim3优化
6. 地图回环优化
7. 地图融合优化

下面给出逐步注释分析

2.代码分析

LoopClosing::DetectAndReffineSim3FromLastKF使用
1投2， 2投1这么来 只优化g2oS12

int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2,
                            const bool bFixScale, Eigen::Matrix<double, 7, 7> &mAcumHessian, const bool bAllPoints)
{
    // 1. 初始化g2o优化器
    // 先构造求解器
    g2o::SparseOptimizer optimizer;
    // 构造线性方程求解器，Hx = -b的求解器
    g2o::BlockSolverX::LinearSolverType *linearSolver;
    // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();

    g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
    // 使用L-M迭代
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // Camera poses
    // 2.1 添加Sim3顶点
    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
    ORB_SLAM3::VertexSim3Expmap *vSim3 = new ORB_SLAM3::VertexSim3Expmap();
    vSim3->_fix_scale = bFixScale;
    vSim3->setEstimate(g2oS12);
    vSim3->setId(0);
    vSim3->setFixed(false);
    vSim3->pCamera1 = pKF1->mpCamera;
    vSim3->pCamera2 = pKF2->mpCamera;
    optimizer.addVertex(vSim3);

    // Set MapPoint vertices
    // 2.1 添加MP顶点
    const int N = vpMatches1.size();
    const vector<MapPoint *> vpMapPoints1 = pKF1->GetMapPointMatches();
    vector<ORB_SLAM3::EdgeSim3ProjectXYZ *> vpEdges12;        // pKF2对应的MapPoints到pKF1的投影
    vector<ORB_SLAM3::EdgeInverseSim3ProjectXYZ *> vpEdges21; // pKF1对应的MapPoints到pKF2的投影
    vector<size_t> vnIndexEdge;
    vector<bool> vbIsInKF2;

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

    const float deltaHuber = sqrt(th2);

    int nCorrespondences = 0;
    int nBadMPs = 0;         // 没有实际用处，没有输出信息
    int nInKF2 = 0;          // 输出信息用
    int nOutKF2 = 0;         // 输出信息用
    int nMatchWithoutMP = 0; // 输出信息用

    vector<int> vIdsOnlyInKF2;

    // 2.2 遍历当前关键帧的所有MP点
    for (int i = 0; i < N; i++)
    {
        if (!vpMatches1[i])
            continue;

        // pMP1和pMP2是匹配的MapPoints，pMP1表示当前帧正常对应的mp，pMP2表示对应的回环的mp
        MapPoint *pMP1 = vpMapPoints1[i];
        MapPoint *pMP2 = vpMatches1[i];

        // (1, 2) (3, 4) (5, 6)
        const int id1 = 2 * i + 1;
        const int id2 = 2 * (i + 1);

        // 返回这个点在pKF2关键帧中对应的特征点id
        const int i2 = get<0>(pMP2->GetIndexInKeyFrame(pKF2));

        cv::Mat P3D1c; // 点1在当前关键帧相机坐标系下的坐标
        cv::Mat P3D2c; // 点2在候选关键帧相机坐标系下的坐标

        if (pMP1 && pMP2)
        {
            if (!pMP1->isBad() && !pMP2->isBad())
            {
                // 2.3 添加PointXYZ顶点， 且设为了固定
                g2o::VertexSBAPointXYZ *vPoint1 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D1w = pMP1->GetWorldPos();
                P3D1c = R1w * P3D1w + t1w;
                vPoint1->setEstimate(Converter::toVector3d(P3D1c)); // 点1在当前关键帧下的三维点坐标
                vPoint1->setId(id1);
                vPoint1->setFixed(true);
                optimizer.addVertex(vPoint1);

                g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D2w = pMP2->GetWorldPos();
                P3D2c = R2w * P3D2w + t2w;
                vPoint2->setEstimate(Converter::toVector3d(P3D2c)); // 点2在候选关键帧下的三维点坐标
                vPoint2->setId(id2);
                vPoint2->setFixed(true);
                optimizer.addVertex(vPoint2);
            }
            else
            {
                nBadMPs++;
                continue;
            }
        }
        else
        {
            nMatchWithoutMP++;

            // The 3D position in KF1 doesn't exist
            if (!pMP2->isBad())
            {
                // 执行到这里意味着特征点没有对应的原始MP，却有回环MP，将其投到候选帧里面
                g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D2w = pMP2->GetWorldPos();
                P3D2c = R2w * P3D2w + t2w;
                vPoint2->setEstimate(Converter::toVector3d(P3D2c));
                vPoint2->setId(id2);
                vPoint2->setFixed(true);
                optimizer.addVertex(vPoint2);

                vIdsOnlyInKF2.push_back(id2);
            }
            continue;
        }

        if (i2 < 0 && !bAllPoints) // bAllPoints = true
        {
            Verbose::PrintMess("    Remove point -> i2: " + to_string(i2) + "; bAllPoints: " + to_string(bAllPoints), Verbose::VERBOSITY_DEBUG);
            continue;
        }

        if (P3D2c.at<float>(2) < 0)
        {
            Verbose::PrintMess("Sim3: Z coordinate is negative", Verbose::VERBOSITY_DEBUG);
            continue;
        }

        nCorrespondences++;

        // 2.4 添加两个顶点（3D点）到相机投影的边
        // 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;

        // 这个边的误差计算方式
        // 1. 将点2通过g2oS12计算到当前关键帧下
        // 2. 点2在当前关键帧下投影到图像上与观测求误差
        ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ();

        e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id2))); // 2相机坐标系下的三维点
        e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));   // g2oS12
        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
        // 2.5 另一个边
        Eigen::Matrix<double, 2, 1> obs2;
        cv::KeyPoint kpUn2;
        bool inKF2;
        // 投之前要确定下这个点的像素坐标
        if (i2 >= 0)
        {
            kpUn2 = pKF2->mvKeysUn[i2];
            obs2 << kpUn2.pt.x, kpUn2.pt.y;
            inKF2 = true;

            nInKF2++; // 输出信息，表示在kf2中找到MP2的点数
        }
        else // BUG 如果没找到，使用三维点投影到KF2中，表示并没有特征点与之对应（把这个结果当做obs2是不是会带来一些误差，而且还不通过内参吗？？？，重大bug）
        {
            float invz = 1 / P3D2c.at<float>(2);
            float x = P3D2c.at<float>(0) * invz;
            float y = P3D2c.at<float>(1) * invz;

            // float u = pKF2->fx * x + pKF2->cx;
            // float v = pKF2->fy * y + pKF2->cy;
            // obs2 << u, v;
            // kpUn2 = cv::KeyPoint(cv::Point2f(u, v), pMP2->mnTrackScaleLevel);
            obs2 << x, y;
            kpUn2 = cv::KeyPoint(cv::Point2f(x, y), pMP2->mnTrackScaleLevel);

            inKF2 = false;
            nOutKF2++;
        }

        // 1相机坐标系下的三维点经过g2oS12投影到kf2下计算重投影误差
        ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = new ORB_SLAM3::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);

        vbIsInKF2.push_back(inKF2);
    }

    // Optimize!
    // 3. 开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(5);

    // Check inliers
    // 4.剔除一些误差大的边，因为e12与e21对应的是同一个三维点，所以只要有一个误差太大就直接搞掉
    // Check inliers
    // 进行卡方检验，大于阈值的边剔除，同时删除鲁棒核函数
    int nBad = 0;
    int nBadOutKF2 = 0;
    for (size_t i = 0; i < vpEdges12.size(); i++)
    {
        ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i];
        ORB_SLAM3::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<ORB_SLAM3::EdgeSim3ProjectXYZ *>(NULL);
            vpEdges21[i] = static_cast<ORB_SLAM3::EdgeInverseSim3ProjectXYZ *>(NULL);
            nBad++;

            if (!vbIsInKF2[i])
            {
                nBadOutKF2++;
            }
            continue;
        }

        // Check if remove the robust adjustment improve the result
        e12->setRobustKernel(0);
        e21->setRobustKernel(0);
    }

    // 如果有坏点，迭代次数更多
    int nMoreIterations;
    if (nBad > 0)
        nMoreIterations = 10;
    else
        nMoreIterations = 5;

    if (nCorrespondences - nBad < 10)
        return 0;

    // Optimize again only with inliers
    // 5. 再一次优化
    optimizer.initializeOptimization();
    optimizer.optimize(nMoreIterations);

    int nIn = 0;
    mAcumHessian = Eigen::MatrixXd::Zero(7, 7);
    // 更新vpMatches1，删除外点，统计内点数量
    for (size_t i = 0; i < vpEdges12.size(); i++)
    {
        ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i];
        ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = vpEdges21[i];
        if (!e12 || !e21)
            continue;

        e12->computeError();
        e21->computeError();

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

    // Recover optimized Sim3、
    // 6.得到优化后的结果
    g2o::VertexSim3Expmap *vSim3_recov = static_cast<g2o::VertexSim3Expmap *>(optimizer.vertex(0));
    g2oS12 = vSim3_recov->estimate();

    return nIn;
}