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

最新推荐文章于 2024-07-23 16:01:39 发布
最新推荐文章于 2024-07-23 16:01:39 发布
阅读量804 收藏 2
点赞数
分类专栏： SLAM 文章标签： 其他
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/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;
}
口哨糖youri
关注
  • 0
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
【逐函数讲解ORB_SLAM3源码】获取普通帧率、关键帧、IMU数据
小秋 AI SLAM 入门实战教程
05-12 804
获得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源码解读】EuRoC双目数据集跑通ORB_SLAM2
小秋 AI SLAM 入门实战教程
04-08 3459
Tracking-跟踪Local Mapping-地图Loop Closing-回环ORB_SLAM2与传感器之间是相互独立，提取特征之后，图像不需要被保存而是直接丢弃。ORB_SLAM2将编译出libORB_SLAM2 . so安装在lib目录中并且生成可执行的数据集测试样例。
ORB_SLAM3与VINS_MONO的惯导部分比较以及尺度因子优化分析
Goretzka的博客
02-26 2804
inertial部分的对比 ORB_SLAM3 VINS IMU的偏差 同时考虑了加速度计和陀螺仪的偏差，并且将其放在目标函数中与重力方向，尺度因子等待优化变量共同优化，在inertialoptimization函数 只估计了陀螺仪的偏差，并且是将其考虑成非线性的，将其独立出来单独优化，没有采用g2o求解器，而是直接一次高斯牛顿迭代解决，在initial_aligment.cpp中的solveGyroscopeBias中。 速度，重力，尺度因子 与IMU偏差共同估计，通过最大后验估计
《ORB-SLAM3》IMU初始化
jjjqqq123321的博客
01-23 1363
刚接触了IMU传感器，除了IMU预积分的相关知识外，关于IMU的另一个重要知识点就是IMU的初始化。IMU为什么需要初始化？IMU初始化是为了获得重力方向和IMU零偏的初始值。有了正确的重力方向才能消除IMU预积分中加速度计关于重力的影响，得到的IMU预积分数据才能保证准确。
重磅直播 | ORB-SLAM3系列代码讲解：IMU 专题三
3D视觉工坊
11-08 411
ORB-SLAM3中的IMU预积分
qq_49561752的博客
04-13 712
IMU预积分之前也博客也提到几次了，这次把ORB-SLAM3中相应的流程记录下，与推导公式一一对应，顺带复习。
ORB-SLAM2项目实战(3) — ORB-SLAM2稠密地图重建
几度春风里的博客
06-04 7676
文章参考部分开源代码和报错文章。
SLAM ORB-SLAM2（26）重定位过程
氢键H-H
05-31 8011
先介绍了词袋模型的基本思想，然后介绍了重定位的详细过程：1. 计算当前帧特征点的词袋向量；2. 根据用词袋查找相似的候选关键帧；3. 通过词袋模型进行初步匹配；4. 查询较匹配的关键帧（通过PnP投影估计姿态>对内点进行BA优化位姿>不够数就通过投影方式寻找更多匹配点>找到较匹配的关键帧）
ORB-SLAM2算法10之图像关键帧KeyFrame
重点记录机器人和自动驾驶相关领域的技术，尤其是计算机视觉，视觉SLAM等，不仅有前言文献或必备算法的学习，也会有实际应用的点滴经验
08-23 424
学习ORB-SLAM2中的图像关键帧，也就是KeyFrame类，该类中主要包含关键帧姿态计算、关键帧之间共视图、生成树及地图点操作等函数。
ORB-SLAM2项目实战(1) — ORB_SLAM2安装与编译报错的解决办法
几度春风里的博客
11-12 5629
ORB_SLAM2安装与编译报错的解决办法
ORB-SLAM笔记——ORB-SLAM3的IMU初始化（1）理论部分
RoBOt__Dreamer
07-28 7929
上一篇blog立的flag，哎最近工作虽然也是快忙炸了但面对这么好的一个作品，你就忍不住挤时间也要学他啊哈哈。那么我们开始！ 在正式说它的IMU初始化部分之前，还是跟随作者论文的思路，咱们再把Visual-Inertial的架构梳理一下。 首先对于一个纯视觉的系统来说，单目也好，双目也罢，我们主要关心的还是每一个pose的6Dof，对应的状态空间呢也就是六维。考虑到单目的尺度问题，状态空间也就是多了一个scale。但在一个visual-inertial SLAM系统中，我们在对当前pose进行估计的同时
ORBSLAM3的IMU积分求解过程
10-20 3977
目前是自己的手稿版的ORBSLAM3的最大后验概率推导, 主要是参考论文: <Inertial-Only Optimization for Visual-Inertial Initialization> 后面找到大块的时间编辑一下, 大家可以先收藏,然后互相交流~~
【SLAM学习笔记】7-ORB_SLAM3关键源码分析⑤ Optimizer（二）局部地图优化
qq_47891062的博客
11-07 918
2021SC@SDUSC
重磅直播 | ORB-SLAM3系列代码讲解：IMU 专题二
3D视觉工坊
09-17 347
ORBSLAM3初始化对配置文件里的IMU标定参数（噪声和随机游走）这么敏感，是拿那些参数参与实际的IMU预积分运算了
最新发布
诗筱涵的博客
07-23 753
Nga 和NgaWalk 有在Preintegrated::IntegrateNewMeasurement函数里更新协方差的时候用到，协方差也就是信息矩阵，程小六的书里有讲（这感觉跟vins里面影响IMU预积分权重类似啊 ），Preintegrated::IntegrateNewMeasurement函数就是预积分计算函数（可以看这里的注释。ORBSLAM3初始化对配置文件里的IMU标定参数（噪声和随机游走）这么敏感，是拿那些参数参与实际的IMU预积分运算了，vins里面那些参数只是影响IMU预积分权重。
【转载】ORB-SLAM3源码阅读笔记7：对IMU异常状态的进一步分析
我才是一卓的博客
08-25 1070
最后，如下图所示，我们总结了ORB-SLAM3中IMU可能出现的异常状态以及其对应的变量、调用函数和后续步骤。
ORBSLAM3 IMU理论推导部分
qq_41694024的博客
03-29 2290
ORBSLAM3 IMU理论推导部分
ORB-SLAM3测试：数据集（单目/双目/imu）& ROS (D435 T265)
dueen1123的博客
10-12 7428
orb-slam3
SLAM学习日志：ORBSLAM3(VIO/IMU)适配KITTI数据集，并评估！！！奶妈级教程！！！
热门推荐
a922922737的博客
03-23 1万+
可能是全网首个相关blog喔！ 准备好进入填坑之旅了吗？ 其实这些坑填完，也可以在kitti上跑各种VIO算法了。 ——————————分割线———————————
ORB-SLAM2源码编译与TUM RGB-D数据集实战
"ORB-SLAM2 技术详解(3)– 源码编译及TUM RGB-D数据集测试" ORB-SLAM2 是一个强大的视觉SLAM（Simultaneous Localization And Mapping，同时定位与建图）系统，支持单目、双目和RGB-D相机。该系统在SLAM领域具有...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

打赏作者

口哨糖youri

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

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

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

打赏作者

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

抵扣说明：

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

余额充值