ORB-SLAM3源码的学习：Atlas.cc②: Atlas:: CreateNewMap创建新地图

news2025/2/11 10:34:47

前言

简单总结一下地图是何时创建的：

构建slam系统时还没有地图就需要创建，当时间戳不对劲时影响数据的同步时需要创建，当跟踪的第一和第二阶段都为失败时都要分别创建，且满足一定要求的地图会保留作为非活跃地图。

1.创建新地图

1.存在当前活跃地图，则先将当前活跃地图标记为非活跃地图。 

2.新建地图，将新地图标记为活跃地图。 

3.将新的活跃地图插入地图集中。 

函数 

注意：将当前地图存储起来，在代码中并未真的进行存储而是把mIsInUse标记为false即将地图设置为非活跃地图，此时的这个非活跃地图仍然在地图集中。

/**
 * @brief 创建新地图，如果当前活跃地图有效，先存储当前地图为不活跃地图，然后新建地图；否则，可以直接新建地图。
 * 
 */
void Atlas::CreateNewMap()
{
    // 锁住地图集
    unique_lock<mutex> lock(mMutexAtlas);//使用互斥锁来保护地图集的线程安全。
    cout << "Creation of new map with id: " << Map::nNextId << endl;//打印新地图创建信息,显示了新地图的ID。
    // 如果当前活跃地图有效，先存储当前地图为不活跃地图后退出
    if (mpCurrentMap)
    {
        // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1
        if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
            mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; // 初始化关键帧ID为当前最大ID的下一个

        // 将当前地图储存起来，其实就是把mIsInUse标记为false
        mpCurrentMap->SetStoredMap();
        cout << "Stored map with ID: " << mpCurrentMap->GetId() << endl;

        // if(mHasViewer)
        //     mpViewer->AddMapToCreateThumbnail(mpCurrentMap);
    }
    cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl;

    mpCurrentMap = new Map(mnLastInitKFidMap);  //新建地图
    mpCurrentMap->SetCurrentMap();              //设置为活跃地图
    mspMaps.insert(mpCurrentMap);               //插入地图集
}

2.创建地图的时机

创建太频繁会影响效率，创建太保守影响效果。 

1.构建SLAM系统时 

在构建地图集时，地图集是空的，这时候需要创建第一个地图。 在system.cc中：

mpAtlas = new Atlas(0);

Atlas类的构造函数则是在Atlas.cc中：

Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid), mHasViewer(false)
{
    mpCurrentMap = static_cast<Map *>(NULL);
    CreateNewMap();
}

2.跟踪线程中时间戳异常时 

ORB-SLAM2 不需要像 ORB-SLAM3 那样专门处理时间戳异常问题，主要是因为它面向的是较为简单的单传感器或双目传感器配置，时间戳同步的问题相对较少。而 ORB-SLAM3 由于支持更复杂的传感器配置（如 IMU、多相机等），因此需要更加精确地处理时间戳同步，以确保不同传感器数据的协调和正确的融合。

异常的两种情况： 

1.时间戳颠倒。当前帧的时间戳比上一帧的时间戳小。 

2.时间戳跳变。当前帧的时间戳与上一帧的时间戳间隔时间比较久（大于1s） 

// Step 2 处理时间戳异常的情况
    if(mState!=NO_IMAGES_YET)
    {
        if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp)
        {
            // 如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图
            cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
            unique_lock<mutex> lock(mMutexImuQueue);
            // mlQueueImuData.clear();
            // 创建新地图
            CreateMapInAtlas();
            return;
        }
        else if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0)
        {
            // cout << mCurrentFrame.mTimeStamp << ", " << mLastFrame.mTimeStamp << endl;
            // cout << "id last: " << mLastFrame.mnId << "    id curr: " << mCurrentFrame.mnId << endl;
            // 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了，重置地图后直接返回
            //根据是否是imu模式,进行imu的补偿
            if(mpAtlas->isInertial())
            {
                // 如果当前地图imu成功初始化
                if(mpAtlas->isImuInitialized())
                {
                    cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
                    // IMU完成第3次初始化（在localmapping线程里）
                    if(!pCurrentMap->GetIniertialBA2())
                    {
                        // 如果当前子图中imu没有经过BA2，重置active地图，也就是之前的数据不要了
                        mpSystem->ResetActiveMap();
                    }
                    else
                    {
                        // 如果当前子图中imu进行了BA2，重新创建新的子图，保存当前地图
                        CreateMapInAtlas();
                    }
                }
                else
                {
                    // 如果当前子图中imu还没有初始化，重置active地图
                    cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;
                    mpSystem->ResetActiveMap();
                }
                return;
            }

        }
    }

其中调用了CreateMapInAtlas函数不仅创建了新地图还重置了系统状态以便重新获取新数据。

具体代码分析：

1.重置初始化帧 ID

mnLastInitFrameId = mCurrentFrame.mnId;

 将当前帧的 ID 记录到mnLastInitFrameId中，标记当前帧作为新地图的起始帧。

2. 创建新地图

调用CreateNewMap函数创建新地图，此时该地图为活跃地图，并将其插入到地图集中。

mpAtlas->CreateNewMap();

3.设置惯性传感器标志

if (mSensor == System::IMU_STEREO || mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_RGBD)
    mpAtlas->SetInertialSensor();  // mpAtlas中map的mbIsInertial=true

如果是IMU的模式是需要包含惯性数据的。

4. 重置初始化标志

mbSetInit = false;  // 好像没什么用

避免后续的重复初始化

5. 设置新地图的初始化帧 ID

mnInitialFrameId = mCurrentFrame.mnId + 1;

6.重置系统状态

mState = NO_IMAGES_YET;

7.重置其他相关标志

mbVelocity = false;

8.打印信息

Verbose::PrintMess("First frame id in map: " + to_string(mnLastInitFrameId + 1), Verbose::VERBOSITY_NORMAL);

 打印日志信息，告知当前新地图中的第一帧 ID。这有助于调试和查看系统的状态。

9.初始化 VO 标志

mbVO = false; // Init value for know if there are enough MapPoints in the last KF

将 mbVO（Visual Odometry）标志设置为 false，表示视觉里程计的状态尚未建立。mbVO 可能用于判断当前的地图中是否有足够的地图点（MapPoints）来支持视觉里程计的估计。

10. 重置单目初始化标志

if (mSensor == System::MONOCULAR || mSensor == System::IMU_MONOCULAR)
{
    mbReadyToInitializate = false;
}

如果当前系统使用的是单目传感器或单目 IMU，mbReadyToInitializate 将被重置为 false。这是因为在单目系统中，初始化过程需要根据多帧图像来恢复 3D 位姿，当前的帧在重新创建地图时可能还没有足够的信息来初始化系统。

11. 重新初始化 IMU 数据

if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpImuPreintegratedFromLastKF)
{
    delete mpImuPreintegratedFromLastKF;
    mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
}

12.重置关键帧指针

if (mpLastKeyFrame)
    mpLastKeyFrame = static_cast<KeyFrame*>(NULL);

if (mpReferenceKF)
    mpReferenceKF = static_cast<KeyFrame*>(NULL);

13. 重置当前和上一帧

mLastFrame = Frame();
mCurrentFrame = Frame();

14.清空匹配点和 IMU 数据队列

mvIniMatches.clear();
mlQueueImuData.clear();

15. 标记地图已创建

mbCreatedMap = true;

完整的代码： 


/*
 在Atlas中保存当前地图，创建新地图，所有跟状态相关的变量全部重置
 1. 前后两帧对应的时间戳反了
 2. imu模式下前后帧超过1s
 3. 上一帧为最近丢失且重定位失败时
 4. 重定位成功，局部地图跟踪失败
*/
void Tracking::CreateMapInAtlas()
{
    mnLastInitFrameId = mCurrentFrame.mnId;
    mpAtlas->CreateNewMap();
    if (mSensor==System::IMU_STEREO || mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_RGBD)
        mpAtlas->SetInertialSensor();  // mpAtlas中map的mbIsInertial=true
    mbSetInit=false;  // 好像没什么用

    mnInitialFrameId = mCurrentFrame.mnId+1;
    mState = NO_IMAGES_YET;

    // Restart the variable with information about the last KF
    mbVelocity = false;
    //mnLastRelocFrameId = mnLastInitFrameId; // The last relocation KF_id is the current id, because it is the new starting point for new map
    Verbose::PrintMess("First frame id in map: " + to_string(mnLastInitFrameId+1), Verbose::VERBOSITY_NORMAL);
    mbVO = false; // Init value for know if there are enough MapPoints in the last KF
    if(mSensor == System::MONOCULAR || mSensor == System::IMU_MONOCULAR)
    {
        mbReadyToInitializate = false;
    }

    if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpImuPreintegratedFromLastKF)
    {
        delete mpImuPreintegratedFromLastKF;
        mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
    }

    if(mpLastKeyFrame)
        mpLastKeyFrame = static_cast<KeyFrame*>(NULL);

    if(mpReferenceKF)
        mpReferenceKF = static_cast<KeyFrame*>(NULL);

    mLastFrame = Frame();
    mCurrentFrame = Frame();
    mvIniMatches.clear();
    mlQueueImuData.clear();

    mbCreatedMap = true;
}

3.跟踪线程中确定跟踪丢失后

根据跟踪的阶段不同，判断条件也不同。如果在跟踪的第一阶段就确定跟踪丢失，则进行如下处理：

1.如果当前活跃地图中关键帧的数量小于10 个，则认为该地图中有效信息太少， 直接重置， 丢弃当前地图。

2.如果当前活跃地图中关键帧的数量超过10 个，则认为该地图仍有一定价值，存储起来作为非活跃地图， 然后新建一个地图。

else if (mState == LOST)  // 上一帧为最近丢失且重定位失败时
                {
                    // Step 6.6 如果是LOST状态
                    // 开启一个新地图
                    Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);

                    if (pCurrentMap->KeyFramesInMap()<10)
                    {
                        // 当前地图中关键帧数目小于10，重置当前地图
                        mpSystem->ResetActiveMap();
                        Verbose::PrintMess("Reseting current map...", Verbose::VERBOSITY_NORMAL);
                    }else
                        CreateMapInAtlas();  // 当前地图中关键帧数目超过10，创建新地图
                    // 干掉上一个关键帧
                    if(mpLastKeyFrame)
                        mpLastKeyFrame = static_cast<KeyFrame*>(NULL);

                    Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);

                    return;
                }

如果在跟踪的第二阶段确定跟踪丢失，则进行如下处理：

1. 如果当前是纯视觉模式且地图中关键帧的数量超过10个或者在惯性模式下已经完成IMU 第一阶段初始化，则认为该地图仍有一定价值， 存储起来作为非活跃地图，然后新建一个地图。

2. 否则重置， 丢弃当前地图。

// Reset if the camera get lost soon after initialization
        // Step 10 如果第二阶段跟踪失败，跟踪状态为LOST
        if(mState==LOST)
        {
            // 如果地图中关键帧小于10，重置当前地图，退出当前跟踪
            if(pCurrentMap->KeyFramesInMap()<=10)  // 上一个版本这里是5
            {
                mpSystem->ResetActiveMap();
                return;
            }
            if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
                if (!pCurrentMap->isImuInitialized())
                {
                    // 如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪
                    Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET);
                    mpSystem->ResetActiveMap();
                    return;
                }
            // 如果地图中关键帧超过10 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图
            CreateMapInAtlas();

            // 新增加了个return
            return;
        }

 以上就是需要创建地图的几种情况。

结束语

以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。

 

 

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/2296274.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

多头自注意力中的多头作用及相关思考

多头自注意力中的多头作用及相关思考

文章目录 1. num_heads2. pytorch源码演算 1. num_heads 将矩阵的最后一维度进行按照num_heads的方式进行切割矩阵&#xff0c;具体表示如下&#xff1a; 2. pytorch源码演算 pytorch 代码 import torch import torch.nn as nn import torch.nn.functional as Ftorch.set…
阅读更多...
常用的python库-安装与使用

常用的python库-安装与使用

常用的python库函数 yield关键字openslide库openslide库的安装-linuxopenslide的使用openslide对象的常用属性 cv2库numpy库ASAP库-multiresolutionimageinterface库ASAP库的安装ASAP库的使用 concurrent.futures.ThreadPoolExecutorxml.etree.ElementTree库skimage库PIL.Image…
阅读更多...
对接DeepSeek

对接DeepSeek

其实&#xff0c;整个对接过程很简单&#xff0c;就四步&#xff0c;获取key&#xff0c;找到接口文档&#xff0c;接口测试&#xff0c;代码对接。 获取 KEY https://platform.deepseek.com/transactions 直接付款就是了&#xff08;现在官网暂停充值2025年2月7日&#xff0…
阅读更多...
ChatGPT提问技巧：行业热门应用提示词案例-文案写作

ChatGPT提问技巧：行业热门应用提示词案例-文案写作

ChatGPT 作为强大的 AI 语言模型&#xff0c;已经成为文案写作的得力助手。但要让它写出真正符合你需求的文案&#xff0c;关键在于如何与它“沟通”&#xff0c;也就是如何设计提示词&#xff08;Prompt&#xff09;。以下是一些实用的提示词案例&#xff0c;帮助你解锁 ChatG…
阅读更多...
分享如何通过Mq、Redis、XxlJob实现算法任务的异步解耦调度

分享如何通过Mq、Redis、XxlJob实现算法任务的异步解耦调度

一、背景 1.1 产品简介 基于大模型塔斯&#xff0c;整合传统的多项能力&#xff08;NLP、OCR、CV等&#xff09;&#xff0c;构建以场景为中心的新型智能文档平台。通过文档审阅&#xff0c;实现结构化、半结构化和非结构化文档的信息获取、处理及审核&#xff0c;同时基于大…
阅读更多...
8.flask+websocket

8.flask+websocket

http是短连接&#xff0c;无状态的。 websocket是长连接&#xff0c;有状态的。 flask中使用websocket from flask import Flask, request import asyncio import json import time import websockets from threading import Thread from urllib.parse import urlparse, pars…
阅读更多...
【大模型实战】使用Ollama+Chatbox实现本地Deepseek R1模型搭建

【大模型实战】使用Ollama+Chatbox实现本地Deepseek R1模型搭建

下载安装Ollama Ollama官方链接：https://ollama.com/，打开链接后就可以看到大大的下载按钮，如下图： 我选择用Win的安装。将Ollama的安装包下载到本地，如果下载慢可以复制链接到迅雷里面，提高下载速度，如下图： 双击之后，就可以开始安装了，如下图： 默认安装到C盘，…
阅读更多...
VMware 虚拟机 ubuntu 20.04 扩容工作硬盘

VMware 虚拟机 ubuntu 20.04 扩容工作硬盘

一、关闭虚拟机 关闭虚拟机参考下图&#xff0c;在vmware 调整磁盘容量 二、借助工具fdisk testubuntu ~ $ df -h Filesystem Size Used Avail Use% Mounted on udev 1.9G 0 1.9G 0% /dev tmpfs 388M 3.1M 385M 1% /run /dev/sda5 …
阅读更多...
【漫话机器学习系列】082.岭回归（或脊回归）中的α值（alpha in ridge regression）

【漫话机器学习系列】082.岭回归（或脊回归）中的α值（alpha in ridge regression）

岭回归&#xff08;Ridge Regression&#xff09;中的 α 值 岭回归&#xff08;Ridge Regression&#xff09;是一种 带有 L2​ 正则化 的线性回归方法&#xff0c;用于处理多重共线性&#xff08;Multicollinearity&#xff09;问题&#xff0c;提高模型的泛化能力。其中&am…
阅读更多...
9 Pydantic复杂数据结构的处理

9 Pydantic复杂数据结构的处理

在构建现代 Web 应用时&#xff0c;我们往往需要处理复杂的输入和输出数据结构。例如&#xff0c;响应数据可能包含嵌套字典、列表、元组&#xff0c;甚至是多个嵌套对象。Pydantic 是一个强大的数据验证和序列化库&#xff0c;可以帮助我们轻松地处理这些复杂的数据结构&#…
阅读更多...
Day62_补20250210_图论part6_108冗余连接|109.冗余连接II

Day62_补20250210_图论part6_108冗余连接|109.冗余连接II

Day62_20250210_图论part6_108冗余连接|109.冗余连接II 108冗余连接 【把题意转化为并查集问题】 题目 有一个图&#xff0c;它是一棵树&#xff0c;他是拥有 n 个节点&#xff08;节点编号1到n&#xff09;和 n - 1 条边的连通无环无向图&#xff08;其实就是一个线形图&am…
阅读更多...
kafka消费端之消费者协调器和组协调器

kafka消费端之消费者协调器和组协调器

文章目录 概述回顾历史老版本获取消费者变更老版本存在的问题 消费者协调器和组协调器新版如何解决老版本问题再均衡过程**第一阶段CFIND COORDINATOR****第二阶段&#xff08;JOINGROUP&#xff09;**选举消费组的lcader选举分区分配策略 第三阶段&#xff08;SYNC GROUP&…
阅读更多...
IDEA升级出现问题Failed to prepare an update Temp directory inside installation

IDEA升级出现问题Failed to prepare an update Temp directory inside installation

IDEA升级出现问题"Failed to prepare an update Temp directory inside installation…" 问题来源&#xff1a; 之前修改了IDEA的默认配置文件路径&#xff0c;然后升级新版本时就无法升级&#xff0c;提示"Failed to prepare an update Temp directory insid…
阅读更多...
十款开源的论坛建站工具

十款开源的论坛建站工具

以下是十款开源的论坛建站工具&#xff0c;它们各具特色&#xff0c;能够满足不同用户的需求&#xff1a; Discuz!&#xff08;Crossday Discuz! Board&#xff09; 特点&#xff1a;基础架构采用web编程组合PHPMySQL&#xff0c;用户可以在不需要任何编程的基础上&#xff0c;…
阅读更多...
vue学习6

vue学习6

1. 智慧商城 1. 路由设计配置 单个页面&#xff0c;独立展示的&#xff0c;是一级路由 2.二级路由配置 规则&组件配置导航链接配置路由出口 <template><div id"app"><!--二级路由出口--><router-view></router-view><van-…
阅读更多...
线程池以及日志、线程总结

线程池以及日志、线程总结

一、线程池以及日志 1、基础线程池写法 主线程在main函数中构建一个线程池&#xff0c;初始化(Init)后开始工作(Start) 此时线程池中每个线程都已经工作起来了&#xff0c;只是任务队列中任务为空&#xff0c;所有线程处于休眠状态(通过线程同步中的条件变量实现&#xff0c…
阅读更多...
Vue 响应式渲染 - 过滤应用

Vue 响应式渲染 - 过滤应用

Vue 渐进式JavaScript 框架 基于Vue2的学习笔记 - Vue响应式渲染综合 - 过滤应用 目录 过滤应用 引入vue Vue设置 设置页面元素 模糊查询过滤实现 函数表达式实现 总结 过滤应用 综合响应式渲染做一个输入框&#xff0c;用来实现&#xff1b;搜索输入框关键词符合列表。…
阅读更多...
【ThreeJS Basics 1-3】Hello ThreeJS，实现第一个场景

【ThreeJS Basics 1-3】Hello ThreeJS，实现第一个场景

文章目录 环境创建一个项目安装依赖基础 Web 页面概念解释编写代码运行项目 环境 我的环境是 node version 22 创建一个项目 首先&#xff0c;新建一个空的文件夹&#xff0c;然后 npm init -y , 此时会快速生成好默认的 package.json 安装依赖 在新建的项目下用 npm 安装依…
阅读更多...
深入理解动态代理

深入理解动态代理

为什么需要动态代理 对于代码的增强逻辑我们是清楚具体实现的，一种方式是增强逻辑作为委托类，被其他业务类调用， 这样会有很多重复代码，而且，当需要根据动态参数来决定增强逻辑时，重复代码会更多，逻辑会更不清晰 二，也是动态代理产生的原始需求，解决类爆照问题， 所以…
阅读更多...
Cherry Studio之DeepSeek联网/本地，建属于自己的AI助理！

Cherry Studio之DeepSeek联网/本地，建属于自己的AI助理！

上一篇文章&#xff0c;讲了DeepSeek-R1部署到本地的方法。这一篇文章&#xff0c;我们让DeepSeek再一次升级&#xff0c;通过图形化界面来交互&#xff0c;从而变成我们的AI助理&#xff0c;让DeepSeek R1发挥最大实力&#xff01; 首选需要借助硅基流动的API接口&#xff0c…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023