【SLAM学习笔记】9-ORB_SLAM3关键源码分析⑦ Optimizer（四）尺度与重力优化

最新推荐文章于 2024-07-23 16:01:39 发布
最新推荐文章于 2024-07-23 16:01:39 发布
阅读量886 收藏 2
点赞数
分类专栏： SLAM 文章标签： 其他
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/qq_47891062/article/details/121461924
版权

2021SC@SDUSC

目录

1.前言

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

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

下面给出逐步注释分析

2.代码分析

imu初始化优化，LocalMapping::InitializeIMU中使用，其中kf的位姿是固定不变的

void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba,
                                        bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA)
{
    Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL);
    int its = 200; // Check number of iterations
    long unsigned int maxKFid = pMap->GetMaxKFid();
    const vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames(); // 获取所有关键帧

    // Setup optimizer
    // 1. 构建优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolverX::LinearSolverType *linearSolver;

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

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

    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

    if (priorG != 0.f)
        solver->setUserLambdaInit(1e3);

    optimizer.setAlgorithm(solver);

    // Set KeyFrame vertices (fixed poses and optimizable velocities)
    // 2. 确定关键帧节点（锁住的位姿及可优化的速度）
    for (size_t i = 0; i < vpKFs.size(); i++)
    {
        KeyFrame *pKFi = vpKFs[i];
        // 跳过id大于当前地图最大id的关键帧
        if (pKFi->mnId > maxKFid)
            continue;
        VertexPose *VP = new VertexPose(pKFi); // 继承于public g2o::BaseVertex<6, ImuCamPose>
        VP->setId(pKFi->mnId);
        VP->setFixed(true);
        optimizer.addVertex(VP);

        VertexVelocity *VV = new VertexVelocity(pKFi); // 继承于public g2o::BaseVertex<3, Eigen::Vector3d>
        VV->setId(maxKFid + (pKFi->mnId) + 1);
        if (bFixedVel)
            VV->setFixed(true);
        else
            VV->setFixed(false);

        optimizer.addVertex(VV);
    }

    // Biases
    // 3. 确定偏置节点，陀螺仪与加速度计
    VertexGyroBias *VG = new VertexGyroBias(vpKFs.front());
    VG->setId(maxKFid * 2 + 2);
    if (bFixedVel)
        VG->setFixed(true);
    else
        VG->setFixed(false);
    optimizer.addVertex(VG);
    VertexAccBias *VA = new VertexAccBias(vpKFs.front());
    VA->setId(maxKFid * 2 + 3);
    if (bFixedVel)
        VA->setFixed(true);
    else
        VA->setFixed(false);

    optimizer.addVertex(VA);

    // prior acc bias
    // 4. 添加关于加速度计与陀螺仪偏置的边，这两个边加入是保证第一帧的偏置为0
    EdgePriorAcc *epa = new EdgePriorAcc(cv::Mat::zeros(3, 1, CV_32F));
    epa->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
    double infoPriorA = priorA;
    epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity());
    optimizer.addEdge(epa);
    EdgePriorGyro *epg = new EdgePriorGyro(cv::Mat::zeros(3, 1, CV_32F));
    epg->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
    double infoPriorG = priorG;
    epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity());
    optimizer.addEdge(epg);

    // Gravity and scale
    // 5. 添加关于重力方向与尺度的节点
    VertexGDir *VGDir = new VertexGDir(Rwg);
    VGDir->setId(maxKFid * 2 + 4);
    VGDir->setFixed(false);
    optimizer.addVertex(VGDir);
    VertexScale *VS = new VertexScale(scale);
    VS->setId(maxKFid * 2 + 5);
    VS->setFixed(!bMono); // Fixed for stereo case
    optimizer.addVertex(VS);

    // Graph edges
    // IMU links with gravity and scale
    // 6. imu信息链接重力方向与尺度信息
    vector<EdgeInertialGS *> vpei; // 后面虽然加入了边，但是没有用到，应该调试用的
    vpei.reserve(vpKFs.size());
    vector<pair<KeyFrame *, KeyFrame *>> vppUsedKF;
    vppUsedKF.reserve(vpKFs.size()); // 后面虽然加入了关键帧，但是没有用到，应该调试用的
    std::cout << "build optimization graph" << std::endl;

    for (size_t i = 0; i < vpKFs.size(); i++)
    {
        KeyFrame *pKFi = vpKFs[i];

        if (pKFi->mPrevKF && pKFi->mnId <= maxKFid)
        {
            if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid)
                continue;
            if (!pKFi->mpImuPreintegrated)
                std::cout << "Not preintegrated measurement" << std::endl;
            // 到这里的条件是pKFi是好的，并且它有上一个关键帧，且他们的id要小于最大id
            // 6.1 检查节点指针是否为空
            // 将pKFi偏置设定为上一关键帧的偏置
            pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
            g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1);
            g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
            g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1);
            g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2);
            g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3);
            g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4);
            g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5);
            if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
            {
                cout << "Error" << VP1 << ", " << VV1 << ", " << VG << ", " << VA << ", " << VP2 << ", " << VV2 << ", " << VGDir << ", " << VS << endl;

                continue;
            }
            // 6.2 这是一个大边。。。。包含了上面所有信息，注意到前面的两个偏置也做了两个一元边加入
            EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated);
            ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
            ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
            ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
            ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
            ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
            ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
            ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir));
            ei->setVertex(7, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VS));

            vpei.push_back(ei);

            vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi));
            optimizer.addEdge(ei);
        }
    }

    // Compute error for different scales
    std::set<g2o::HyperGraph::Edge *> setEdges = optimizer.edges();

    std::cout << "start optimization" << std::endl;
    optimizer.initializeOptimization();
    optimizer.setVerbose(false);
    optimizer.optimize(its);
    std::cout << "end optimization" << std::endl;

    // 7. 取数
    scale = VS->estimate();

    // Recover optimized data
    // Biases
    VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKFid * 2 + 2));
    VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKFid * 2 + 3));
    Vector6d vb;
    vb << VG->estimate(), VA->estimate();
    bg << VG->estimate();
    ba << VA->estimate();
    scale = VS->estimate();

    IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]);
    Rwg = VGDir->estimate().Rwg;

    cv::Mat cvbg = Converter::toCvMat(bg);

    // Keyframes velocities and biases
    const int N = vpKFs.size();
    for (size_t i = 0; i < N; i++)
    {
        KeyFrame *pKFi = vpKFs[i];
        if (pKFi->mnId > maxKFid)
            continue;

        VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKFid + (pKFi->mnId) + 1));
        Eigen::Vector3d Vw = VV->estimate(); // Velocity is scaled after
        pKFi->SetVelocity(Converter::toCvMat(Vw));

        if (cv::norm(pKFi->GetGyroBias() - cvbg) > 0.01)
        {
            pKFi->SetNewBias(b);
            if (pKFi->mpImuPreintegrated)
                pKFi->mpImuPreintegrated->Reintegrate();
        }
        else
            pKFi->SetNewBias(b);
    }
}

LoopClosing::MergeLocal2 中使用
跟参数最多的那个同名函数不同的地方在于很多节点不可选是否固定，优化的目标有：

  • 速度
  • 偏置
void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG, float priorA)
{
    int its = 200;
    long unsigned int maxKFid = pMap->GetMaxKFid();
    const vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();

    // Setup optimizer
    // 1. 构建优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolverX::LinearSolverType *linearSolver;

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

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

    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    solver->setUserLambdaInit(1e3);

    optimizer.setAlgorithm(solver);

    // Set KeyFrame vertices (fixed poses and optimizable velocities)
    // 2. 构建节点，固定rt
    for (size_t i = 0; i < vpKFs.size(); i++)
    {
        KeyFrame *pKFi = vpKFs[i];
        if (pKFi->mnId > maxKFid)
            continue;
        VertexPose *VP = new VertexPose(pKFi);
        VP->setId(pKFi->mnId);
        VP->setFixed(true);
        optimizer.addVertex(VP);

        VertexVelocity *VV = new VertexVelocity(pKFi);
        VV->setId(maxKFid + (pKFi->mnId) + 1);
        VV->setFixed(false);

        optimizer.addVertex(VV);
    }

    // Biases
    // 3. 第一个关键帧的偏置
    VertexGyroBias *VG = new VertexGyroBias(vpKFs.front());
    VG->setId(maxKFid * 2 + 2);
    VG->setFixed(false);
    optimizer.addVertex(VG);

    VertexAccBias *VA = new VertexAccBias(vpKFs.front());
    VA->setId(maxKFid * 2 + 3);
    VA->setFixed(false);

    optimizer.addVertex(VA);
    // prior acc bias

    // 4. 先验边，让偏置趋向于0
    EdgePriorAcc *epa = new EdgePriorAcc(cv::Mat::zeros(3, 1, CV_32F));
    epa->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
    double infoPriorA = priorA;
    epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity());
    optimizer.addEdge(epa);
    EdgePriorGyro *epg = new EdgePriorGyro(cv::Mat::zeros(3, 1, CV_32F));
    epg->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
    double infoPriorG = priorG;
    epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity());
    optimizer.addEdge(epg);

    // Gravity and scale
    // 5. 注意这里固定了尺度与重力方向，所以只优化了偏置与速度
    VertexGDir *VGDir = new VertexGDir(Eigen::Matrix3d::Identity());
    VGDir->setId(maxKFid * 2 + 4);
    VGDir->setFixed(true);
    optimizer.addVertex(VGDir);
    VertexScale *VS = new VertexScale(1.0);
    VS->setId(maxKFid * 2 + 5);
    VS->setFixed(true); // Fixed since scale is obtained from already well initialized map
    optimizer.addVertex(VS);

    // Graph edges
    // IMU links with gravity and scale
    vector<EdgeInertialGS *> vpei;
    vpei.reserve(vpKFs.size());
    vector<pair<KeyFrame *, KeyFrame *>> vppUsedKF;
    vppUsedKF.reserve(vpKFs.size());

    // 6. 设置残差边
    for (size_t i = 0; i < vpKFs.size(); i++)
    {
        KeyFrame *pKFi = vpKFs[i];

        if (pKFi->mPrevKF && pKFi->mnId <= maxKFid)
        {
            if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid)
                continue;

            pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
            g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1);
            g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
            g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1);
            g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2);
            g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3);
            g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4);
            g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5);
            if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
            {
                cout << "Error" << VP1 << ", " << VV1 << ", " << VG << ", " << VA << ", " << VP2 << ", " << VV2 << ", " << VGDir << ", " << VS << endl;

                continue;
            }
            EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated);
            ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
            ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
            ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
            ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
            ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
            ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
            ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir));
            ei->setVertex(7, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VS));

            vpei.push_back(ei);

            vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi));
            optimizer.addEdge(ei);
        }
    }

    // 7. 优化
    // Compute error for different scales
    optimizer.setVerbose(false);
    optimizer.initializeOptimization();
    optimizer.optimize(its);

    // 8. 取数
    // Recover optimized data
    // Biases
    VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKFid * 2 + 2));
    VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKFid * 2 + 3));
    Vector6d vb;
    vb << VG->estimate(), VA->estimate();
    bg << VG->estimate();
    ba << VA->estimate();

    IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]);

    cv::Mat cvbg = Converter::toCvMat(bg);

    // Keyframes velocities and biases
    const int N = vpKFs.size();
    for (size_t i = 0; i < N; i++)
    {
        KeyFrame *pKFi = vpKFs[i];
        if (pKFi->mnId > maxKFid)
            continue;

        VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKFid + (pKFi->mnId) + 1));
        Eigen::Vector3d Vw = VV->estimate();
        pKFi->SetVelocity(Converter::toCvMat(Vw));

        if (cv::norm(pKFi->GetGyroBias() - cvbg) > 0.01)
        {
            pKFi->SetNewBias(b);
            if (pKFi->mpImuPreintegrated)
                pKFi->mpImuPreintegrated->Reintegrate();
        }
        else
            pKFi->SetNewBias(b);
    }
}

优化重力方向与尺度，LocalMapping::ScaleRefinement()中使用，优化目标有：

  • 重力方向与尺度
void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale)
{
    int its = 10;
    long unsigned int maxKFid = pMap->GetMaxKFid();
    const vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();

    // 1. 构建优化器
    // Setup optimizer
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolverX::LinearSolverType *linearSolver;

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

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

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

    // Set KeyFrame vertices (all variables are fixed)
    // 2. 添加帧节点，其中包括位姿，速度，两个偏置
    for (size_t i = 0; i < vpKFs.size(); i++)
    {
        KeyFrame *pKFi = vpKFs[i];
        if (pKFi->mnId > maxKFid)
            continue;
        VertexPose *VP = new VertexPose(pKFi);
        VP->setId(pKFi->mnId);
        VP->setFixed(true);
        optimizer.addVertex(VP);

        VertexVelocity *VV = new VertexVelocity(pKFi);
        VV->setId(maxKFid + 1 + (pKFi->mnId));
        VV->setFixed(true);
        optimizer.addVertex(VV);

        // Vertex of fixed biases
        VertexGyroBias *VG = new VertexGyroBias(vpKFs.front());
        VG->setId(2 * (maxKFid + 1) + (pKFi->mnId));
        VG->setFixed(true);
        optimizer.addVertex(VG);
        VertexAccBias *VA = new VertexAccBias(vpKFs.front());
        VA->setId(3 * (maxKFid + 1) + (pKFi->mnId));
        VA->setFixed(true);
        optimizer.addVertex(VA);
    }

    // 3. 添加重力方向与尺度的节点，为优化对象
    // Gravity and scale
    VertexGDir *VGDir = new VertexGDir(Rwg);
    VGDir->setId(4 * (maxKFid + 1));
    VGDir->setFixed(false);
    optimizer.addVertex(VGDir);
    VertexScale *VS = new VertexScale(scale);
    VS->setId(4 * (maxKFid + 1) + 1);
    VS->setFixed(false);
    optimizer.addVertex(VS);

    // Graph edges
    // 4. 添加边
    for (size_t i = 0; i < vpKFs.size(); i++)
    {
        KeyFrame *pKFi = vpKFs[i];

        if (pKFi->mPrevKF && pKFi->mnId <= maxKFid)
        {
            if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid)
                continue;

            g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex *VV1 = optimizer.vertex((maxKFid + 1) + pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
            g2o::HyperGraph::Vertex *VV2 = optimizer.vertex((maxKFid + 1) + pKFi->mnId);
            g2o::HyperGraph::Vertex *VG = optimizer.vertex(2 * (maxKFid + 1) + pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex *VA = optimizer.vertex(3 * (maxKFid + 1) + pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(4 * (maxKFid + 1));
            g2o::HyperGraph::Vertex *VS = optimizer.vertex(4 * (maxKFid + 1) + 1);
            if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
            {
                Verbose::PrintMess("Error" + to_string(VP1->id()) + ", " + to_string(VV1->id()) + ", " + to_string(VG->id()) + ", " + to_string(VA->id()) + ", " + to_string(VP2->id()) + ", " + to_string(VV2->id()) + ", " + to_string(VGDir->id()) + ", " + to_string(VS->id()), Verbose::VERBOSITY_NORMAL);

                continue;
            }
            EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated);
            ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
            ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
            ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
            ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
            ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
            ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
            ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir));
            ei->setVertex(7, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VS));

            optimizer.addEdge(ei);
        }
    }
    // 5. 优化
    // Compute error for different scales
    optimizer.setVerbose(false);
    optimizer.initializeOptimization();
    optimizer.optimize(its);

    // Recover optimized data
    // 6. 取数
    scale = VS->estimate();
    Rwg = VGDir->estimate().Rwg;
}
【ORB_SLAM2源码解读】EuRoC双目数据集跑通ORB_SLAM2
小秋 AI SLAM 入门实战教程
04-08 3705
Tracking-跟踪Local Mapping-地图Loop Closing-回环ORB_SLAM2与传感器之间是相互独立，提取特征之后，图像不需要被保存而是直接丢弃。ORB_SLAM2将编译出libORB_SLAM2 . so安装在lib目录中并且生成可执行的数据集测试样例。
ORB-SLAM2项目实战(3) — ORB-SLAM2稠密地图重建
几度春风里的博客
06-04 9524
文章参考部分开源代码和报错文章。
ORB_SLAM3与VINS_MONO的惯导部分比较以及尺度因子优化分析
Goretzka的博客
02-26 3170
inertial部分的对比 ORB_SLAM3 VINS IMU的偏差 同时考虑了加速度计和陀螺仪的偏差，并且将其放在目标函数中与重力方向，尺度因子等待优化变量共同优化，在inertialoptimization函数 只估计了陀螺仪的偏差，并且是将其考虑成非线性的，将其独立出来单独优化，没有采用g2o求解器，而是直接一次高斯牛顿迭代解决，在initial_aligment.cpp中的solveGyroscopeBias中。 速度，重力，尺度因子 与IMU偏差共同估计，通过最大后验估计
《ORB-SLAM3》IMU初始化
jjjqqq123321的博客
01-23 2229
刚接触了IMU传感器，除了IMU预积分的相关知识外，关于IMU的另一个重要知识点就是IMU的初始化。IMU为什么需要初始化？IMU初始化是为了获得重力方向和IMU零偏的初始值。有了正确的重力方向才能消除IMU预积分中加速度计关于重力的影响，得到的IMU预积分数据才能保证准确。
重磅直播 | ORB-SLAM3系列代码讲解：IMU 专题三
3D视觉工坊
11-08 446
ORB-SLAM3中的IMU预积分
qq_49561752的博客
04-13 1022
IMU预积分之前也博客也提到几次了，这次把ORB-SLAM3中相应的流程记录下，与推导公式一一对应，顺带复习。
ORB-SLAM笔记——ORB-SLAM3的IMU初始化（1）理论部分
RoBOt__Dreamer
07-28 8213
上一篇blog立的flag，哎最近工作虽然也是快忙炸了但面对这么好的一个作品，你就忍不住挤时间也要学他啊哈哈。那么我们开始！ 在正式说它的IMU初始化部分之前，还是跟随作者论文的思路，咱们再把Visual-Inertial的架构梳理一下。 首先对于一个纯视觉的系统来说，单目也好，双目也罢，我们主要关心的还是每一个pose的6Dof，对应的状态空间呢也就是六维。考虑到单目的尺度问题，状态空间也就是多了一个scale。但在一个visual-inertial SLAM系统中，我们在对当前pose进行估计的同时
【SLAM学习笔记】6-ORB_SLAM3关键源码分析④ Optimizer（一）单帧优化
qq_47891062的博客
10-31 1228
2021SC@SDUSC
orb-slam3的优化函数
MSysの优快云
04-01 1477
列举总结
ORBSLAM3的IMU积分求解过程
10-20 4081
目前是自己的手稿版的ORBSLAM3的最大后验概率推导, 主要是参考论文: <Inertial-Only Optimization for Visual-Inertial Initialization> 后面找到大块的时间编辑一下, 大家可以先收藏,然后互相交流~~
【SLAM学习笔记】7-ORB_SLAM3关键源码分析⑤ Optimizer（二）局部地图优化
qq_47891062的博客
11-07 1025
2021SC@SDUSC
重磅直播 | ORB-SLAM3系列代码讲解：IMU 专题二
3D视觉工坊
09-17 399
ORBSLAM3初始化对配置文件里的IMU标定参数（噪声和随机游走）这么敏感，是拿那些参数参与实际的IMU预积分运算了
诗筱涵的博客
07-23 1080
Nga 和NgaWalk 有在Preintegrated::IntegrateNewMeasurement函数里更新协方差的时候用到，协方差也就是信息矩阵，程小六的书里有讲（这感觉跟vins里面影响IMU预积分权重类似啊 ），Preintegrated::IntegrateNewMeasurement函数就是预积分计算函数（可以看这里的注释。ORBSLAM3初始化对配置文件里的IMU标定参数（噪声和随机游走）这么敏感，是拿那些参数参与实际的IMU预积分运算了，vins里面那些参数只是影响IMU预积分权重。
【转载】ORB-SLAM3源码阅读笔记7：对IMU异常状态的进一步分析
我才是一卓的博客
08-25 1520
最后，如下图所示，我们总结了ORB-SLAM3中IMU可能出现的异常状态以及其对应的变量、调用函数和后续步骤。
ORBSLAM3 IMU理论推导部分
qq_41694024的博客
03-29 2754
ORBSLAM3 IMU理论推导部分
ORB-SLAM3测试：数据集（单目/双目/imu）& ROS (D435 T265)
dueen1123的博客
10-12 9569
orb-slam3
SLAM学习日志：ORBSLAM3(VIO/IMU)适配KITTI数据集，并评估！！！奶妈级教程！！！
热门推荐
a922922737的博客
03-23 1万+
可能是全网首个相关blog喔！ 准备好进入填坑之旅了吗？ 其实这些坑填完，也可以在kitti上跑各种VIO算法了。 ——————————分割线———————————
orbslam3 单目_imu采坑_断言错误等
Forever223的博客
03-30 5999
orbslam3 断言错误/段错误/opencv版本问题
【逐函数讲解ORB_SLAM3源码】获取普通帧率、关键帧、IMU数据
小秋 AI SLAM 入门实战教程
05-12 898
获得imu的旋转 mRwc * mImuCalib.mTcb.rotationMatrix(); Sophus::SE3 mTwc 创建ORB_SLAM3系统 Create SLAM system. It initializes all system threads and gets ready to process frames. ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_RGBD, true, 0, file_nam
ORB-SLAM2学习笔记
最新发布
03-12
<think>嗯，用户让我介绍一下ORB-SLAM2的学习笔记。我需要先回忆一下ORB-SLAM2的基本框架和核心部分。ORB-SLAM2是一个比较经典的视觉SLAM系统，分为几个主要模块：特征提取、跟踪、局部建图、回环检测和地图管理。首先，我应该从系统架构开始介绍，说明各个模块的作用和相互关系。 接下来，用户可能需要了解ORB特征的相关内容。ORB特征结合了FAST关键点检测和BRIEF描述子，具有旋转不变性和一定的尺度不变性。这部分需要详细解释ORB特征的提取过程，包括如何构建图像金字塔，计算方向，生成描述子等步骤。 然后是跟踪线程，也就是Tracking部分。这里需要说明如何通过帧间匹配、运动模型或重定位来估计相机位姿。可能需要提到初始化的过程，比如使用单应矩阵或基础矩阵来恢复运动，以及如何三角化初始地图点。 局部建图模块负责处理新的关键帧，剔除冗余点，进行局部BA优化。这部分需要解释BA（Bundle Adjustment）的概念，以及如何优化相机位姿和地图点。同时，还要提到关键帧的选择策略，比如根据共视点数量或时间间隔来决定是否插入新的关键帧。 回环检测和地图融合部分也是重点。这里需要讲DBoW2的词袋模型，如何通过视觉词典检测回环，然后进行Sim3优化来校正漂移。此外，还要说明全局BA优化在回环校正后的作用，以及如何优化整个地图的一致性。 ORB-SLAM2支持单目、双目和RGB-D相机，需要比较不同传感器下的系统表现。比如单目初始化困难，存在尺度不确定性，而双目和RGB-D可以直接获取深度信息，初始化更简单。 在优缺点分析方面，优点包括实时性高、支持多传感器、具备回环检测和重定位能力。缺点可能有对动态环境敏感，单目模式下尺度问题，以及特征提取在纹理缺失区域效果差。可以提到后续的ORB-SLAM3在IMU融合和多地图系统上的改进。 最后，学习建议部分应该推荐一些资源，比如论文、代码库、书籍和课程。同时建议从基础开始，动手实践，并参与社区讨论。 需要注意的是，用户可能希望了解实际应用中的问题，比如如何调整参数，处理特定场景的挑战，或者如何扩展ORB-SLAM2的功能。不过根据用户的问题，主要是介绍学习笔记，所以需要结构清晰，分点说明，确保覆盖主要模块和关键点。 可能还需要检查LaTeX格式是否正确，比如公式是否用$或$$包裹，变量是否正确。例如，提到BA优化时，可以写目标函数是$$\min \sum \| \pi(X, T) - x \|^2$$，其中$T$是位姿，$X$是地图点。确保这些公式的格式正确。 总之，我需要按照系统架构、特征提取、跟踪、局部建图、回环检测、多传感器支持、优缺点、学习建议的结构来组织内容，使用清晰的中文，正确插入公式，并确保技术细节准确。</think>以下为ORB-SLAM2学习笔记的整理，按模块分阶段说明： --- ### 一、系统架构 $$ \text{ORB-SLAM2} = \text{Tracking} + \text{Local Mapping} + \text{Loop Closing} + \text{Map} $$ 三大线程并行运行： 1. **Tracking线程**：实时估计相机位姿（$T_{cw} \in SE(3)$） 2. **Local Mapping线程**：局部BA优化与地图点维护 3. **Loop Closing线程**：回环检测与全局优化 --- ### 二、ORB特征提取 - **FAST关键点检测**：阈值化像素差异（$|I_p - I_{center}| > \delta$） - **BRIEF描述子**：二进制编码（256-bit，汉明距离匹配） - **改进特性**： - 图像金字塔实现尺度不变性（$n$层缩放，比例因子$s=1.2$） - 灰度质心法计算方向（$m_{10}=\sum xI(x,y), m_{01}=\sum yI(x,y)$） --- ### 三、跟踪线程（Tracking） 1. **初始位姿估计**： - 通过上一帧运动模型预测：$T_{k} = T_{k-1} \cdot T_{k-1}^{k}$ - 或通过特征匹配与PnP求解：$\min_T \sum \| \pi(T \cdot X_i) - x_i \|^2$ 2. **关键帧判定**： - 跟踪质量下降（匹配点 < 阈值） - 距离上一关键帧超过20帧 --- ### 四、局部建图（Local Mapping） 1. **局部BA优化**： $$ \min_{T_i, X_j} \sum \| \pi(T_i \cdot X_j) - x_{ij} \|^2 + \rho(\| T_i^{-1}T_j \|) $$ - 仅优化当前关键帧及其共视关键帧 2. **地图点剔除**： - 观测次数 < 2次 - 重投影误差 > $\chi^2$阈值（$\chi^2_{0.95}=5.991$） --- ### 五、回环检测（Loop Closing） 1. **词袋模型（DBoW2）**： - 离线训练视觉词典（k-means树结构） - 计算相似性得分：$s = \frac{\langle v_q, v_t \rangle}{\|v_q\| \|v_t\|}$ 2. **Sim3优化**： $$ \min_{S} \sum \| \pi(S \cdot X_i) - x_i \|^2 + \| \pi(S^{-1} \cdot X_j&#39;) - x_j&#39; \|^2 $$ - 解决尺度漂移问题（$S \in Sim(3)$） --- ### 六、多传感器支持 | 传感器类型 | 初始化方式 | 深度获取方法 | |------------|---------------------|-----------------------| | 单目 | 分解H/F矩阵 | 三角化（需运动） | | 双目/RGB-D | 直接构造深度图 | 视差/深度传感器 | --- ### 七、优缺点分析 - **优点**： - 实时性（30fps，GTX 1070） - 支持重定位与动态场景处理 - **缺点**： - 依赖特征匹配（纹理缺失易失效） - 单目模式需10秒以上初始化 --- ### 八、学习建议 1. **必读文献**： - 原论文《ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras》 - 《Multiple View Geometry in Computer Vision》 2. **代码实践**： ```bash git clone https://github.com/raulmur/ORB_SLAM2 cd ORB_SLAM2 && chmod +x build.sh && ./build.sh ``` 3. **调试工具**： - 使用g2o_viewer可视化优化结果 - 分析`KeyFrame::EraseMapPointMatch()`等关键函数 --- 以上内容需结合代码阅读（推荐使用CLion或VSCode调试），重点关注`Tracking.cc`和`Optimizer.cc`模块。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

口哨糖youri

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值