关键帧与地图点(一):地图点

本次我们主要讲解ORBSLAM2中的地图点,首先我们来看一下论文中对于地图点的定义:

ORBSLAM2中的每个地图点 p i p_i pi存储了以下内容:

  1. 在世界坐标系下的3D位置 X w , i X_{w,i} Xw,i
  2. 地图点的观测方向 n i n_i ni,为归一化后的平均观测方向(指连接该地图点和其对应观测关键帧光心的单位方向向量的均值)
  3. 一个具有代表性ORB描述子 D i D_i Di,与其他所有能观测到该点云的关键帧中ORB描述子相比,该描述子的汉明距离中值最小
  4. 根据ORB特征点的尺度不变性获取到的可以观察到这个地图点的的最大 d m a x d_{max} dmax和最小 d m i n d_{min} dmin距离。

对照代码,分别为

    // MapPoint在世界坐标系下的3D位置
    cv::Mat mWorldPos; 

    // 观测到该MapPoint的KF和该MapPoint在KF中的索引
    std::map<KeyFrame*,size_t> mObservations; 

    // 该MapPoint的平均观测方向
    // 用于判断点是否在可视范围内
    cv::Mat mNormalVector;

	// 最具代表性的描述子
    // 每个3D点也有一个描述子,但是这个3D点可以观测多个二维特征点,从中选择一个最有代表性的
     //通过 ComputeDistinctiveDescriptors() 得到的最有代表性描述子,距离其它描述子的平均距离最小
    cv::Mat mDescriptor; 

    // 通常情况下MapPoint的参考关键帧就是创建该MapPoint的那个关键帧
    KeyFrame* mpRefKF;

    /// 地图点被跟踪到的次数
    int mnVisible;
    int mnFound;

    /// Bad 标志 (we do not currently erase MapPoint from memory)
    bool mbBad;
    
    //? 替换本地图点的点? 
    MapPoint* mpReplaced;

    /// 尺度不变性距离
    float mfMinDistance;
    float mfMaxDistance;

    ///所属的地图
    Map* mpMap;

    ///对当前地图点位姿进行操作的时候的互斥量
    std::mutex mMutexPos;
    ///对当前地图点的特征信息进行操作的时候的互斥量
    std::mutex mMutexFeatures;

接下来我们需要关注几个和地图点相关的非常重要的函数

  • 计算平均观测方向和尺度不变性距离

    /**
     * @brief 更新地图点的平均观测方向、观测距离范围
     *
     */
    void MapPoint::UpdateNormalAndDepth()
    {
        // Step 1 获得观测到该地图点的所有关键帧、坐标等信息
        map<KeyFrame*,size_t> observations;
        // 地图点的参考关键帧
        KeyFrame* pRefKF;
        // 地图点的3D位置
        cv::Mat Pos;
        {
            // 加锁
            unique_lock<mutex> lock1(mMutexFeatures);
            unique_lock<mutex> lock2(mMutexPos);
    
            // 不处理标记为Bad的点
            if(mbBad)
                return;
    
            observations=mObservations; // 获得观测到该地图点的所有关键帧
            pRefKF=mpRefKF;             // 观测到该点的参考关键帧(第一次创建时的关键帧)
            Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置
        }
    
        // 如果观测为空,则不做处理
        if(observations.empty())
            return;
    
        // Step 2 计算该地图点的平均观测方向
    
        // 能观测到该地图点的所有关键帧,对该点的观测方向归一化为单位向量,然后进行求和得到该地图点的朝向
        // 初始值为0向量,累加为归一化向量,最后除以总数n
        cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
        int n=0;
    
        // 遍历观测到该地图点的所有关键帧
        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            KeyFrame* pKF = mit->first;
            // 获取关键帧对应的相机光心位置
            cv::Mat Owi = pKF->GetCameraCenter();
            // 获得连接地图点和观测到它关键帧的光心的向量并归一化
            cv::Mat normali = mWorldPos - Owi;
            normal = normal + normali/cv::norm(normali);                       
            n++;
        } 
    
        cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量(在世界坐标系下的表示)
        const float dist = cv::norm(PC);                                        // 该点到参考关键帧相机的距离
        const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        // 获取该地图点在参考帧中对应的特征点的金字塔层数
        const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 该金字塔层数对应的尺度因子,scale^n,scale=1.2,n为层数
        const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔总层数,默认为8
    
        {
            unique_lock<mutex> lock3(mMutexPos);
            // 使用方法见PredictScale函数前的注释
            mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
            mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
            mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
        }
    }
    
  • 计算最具代表性的描述子

    /**
     * @brief 计算地图点最具代表性的描述子
     *
     * 由于一个地图点会被许多相机观测到,因此在插入关键帧后,需要判断是否更新代表当前点的描述子 
     * 先获得当前点的所有描述子,然后计算描述子之间的两两距离,最好的描述子与其他描述子应该具有最小的距离中值
     */
    void MapPoint::ComputeDistinctiveDescriptors()
    {
        // 检索所有观测到的描述子
        vector<cv::Mat> vDescriptors;
    
        // 该地图点的所有观测
        map<KeyFrame*,size_t> observations;
    
        // Step 1 获取该地图点所有有效的观测关键帧信息
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            if(mbBad)
                return;
            observations=mObservations;
        }
    
        // 如果观测为空直接返回
        if(observations.empty())
            return;
    
        vDescriptors.reserve(observations.size());
    
        // Step 2 遍历观测到该地图点的所有关键帧,对应的orb描述子,放到向量vDescriptors中
        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            // mit->first取观测到该地图点的关键帧
            // mit->second取该地图点在关键帧中的索引
            KeyFrame* pKF = mit->first;
    
            if(!pKF->isBad())        
                // 取对应的描述子向量                                               
                vDescriptors.push_back(pKF->mDescriptors.row(mit->second));     
        }
    
        if(vDescriptors.empty())
            return;
    
    
        // Step 3 计算这些描述子两两之间的距离
        // N表示为一共多少个描述子
        const size_t N = vDescriptors.size();
    	
        // 将Distances表述成一个对称的矩阵
        // float Distances[N][N];
    	std::vector<std::vector<float> > Distances;
    	Distances.resize(N, vector<float>(N, 0));
    	for (size_t i = 0; i<N; i++)
        {
            // 和自己的距离当然是0
            Distances[i][i]=0;
            // 计算并记录不同描述子距离
            for(size_t j=i+1;j<N;j++)
            {
                int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
                Distances[i][j]=distij;
                Distances[j][i]=distij;
            }
        }
    
    
        // Step 4 选择最有代表性的描述子,它与其他描述子应该具有最小的距离中值
        int BestMedian = INT_MAX;   // 记录最小的中值
        int BestIdx = 0;            // 最小中值对应的索引
        for(size_t i=0;i<N;i++)
        {
            // 第i个描述子到其它所有描述子之间的距离
            // vector<int> vDists(Distances[i],Distances[i]+N);
    		vector<int> vDists(Distances[i].begin(), Distances[i].end());
    
            // 从小到大排序
    		sort(vDists.begin(), vDists.end());
    
            // 获得中值
            int median = vDists[0.5*(N-1)];
            
            // 寻找最小的中值
            if(median<BestMedian)
            {
                BestMedian = median;
                BestIdx = i;
            }
        }
        // 为最具代表性的描述子赋值
        {
            unique_lock<mutex> lock(mMutexFeatures);
            mDescriptor = vDescriptors[BestIdx].clone();       
        }
    }
    
  • 根据地图点距离关键帧/普通帧相机光心的距离来预测地图点对应的描述子应该在哪一个尺度金字塔

    // 下图中横线的大小表示不同图层图像上的一个像素表示的真实物理空间中的大小
    //              ____
    // Nearer      /____\     level:n-1 --> dmin
    //            /______\                       d/dmin = 1.2^(n-1-m)
    //           /________\   level:m   --> d
    //          /__________\                     dmax/d = 1.2^m
    // Farther /____________\ level:0   --> dmax
    //
    //           log(dmax/d)
    // m = ceil(------------)
    //            log(1.2)
    // 这个函数的作用:
    // 在进行投影匹配的时候会给定特征点的搜索范围,考虑到处于不同尺度(也就是距离相机远近,位于图像金字塔中不同图层)的特征点受到相机旋转的影响不同,
    // 因此会希望距离相机近的点的搜索范围更大一点,距离相机更远的点的搜索范围更小一点,所以要在这里,根据点到关键帧/帧的距离来估计它在当前的关键帧/帧中,
    // 会大概处于哪个尺度
    
    /**
     * @brief 预测地图点对应特征点所在的图像金字塔尺度层数
     * 
     * @param[in] currentDist   相机光心距离地图点距离
     * @param[in] pKF           关键帧
     * @return int              预测的金字塔尺度
     */
    int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF)
    {
        // dmax / d
        float ratio;
        {
            unique_lock<mutex> lock(mMutexPos);
            ratio = mfMaxDistance/currentDist;
        }
    
        // 取对数
        int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
        if(nScale<0)
            nScale = 0;
        else if(nScale>=pKF->mnScaleLevels)
            nScale = pKF->mnScaleLevels-1;
    
        return nScale;
    }
    

    这里用到了高中数学中的换底公式,也就是
    m = l o g 1 , 2 d m a x d = l o g e d m a x d l o g e 1.2 m = log_{1,2}^{\frac{d_{max}}{d}} = \frac{log_{e}^{\frac{d_{max}}{d}}}{log_e^{1.2}} m=log1,2ddmax=loge1.2logeddmax

热门文章

暂无图片
编程学习 ·

C语言二分查找详解

二分查找是一种知名度很高的查找算法&#xff0c;在对有序数列进行查找时效率远高于传统的顺序查找。 下面这张动图对比了二者的效率差距。 二分查找的基本思想就是通过把目标数和当前数列的中间数进行比较&#xff0c;从而确定目标数是在中间数的左边还是右边&#xff0c;将查…
暂无图片
编程学习 ·

GMX 命令分类列表

建模和计算操作命令&#xff1a; 1.1 . 创建拓扑与坐标文件 gmx editconf - 编辑模拟盒子以及写入子组(subgroups) gmx protonate - 结构质子化 gmx x2top - 根据坐标生成原始拓扑文件 gmx solvate - 体系溶剂化 gmx insert-molecules - 将分子插入已有空位 gmx genconf - 增加…
暂无图片
编程学习 ·

一文高效回顾研究生课程《数值分析》重点

数值分析这门课的本质就是用离散的已知点去估计整体&#xff0c;就是由黑盒子产生的结果去估计这个黑盒子。在数学里这个黑盒子就是一个函数嘛&#xff0c;这门课会介绍许多方法去利用离散点最大化地逼近这个函数&#xff0c;甚至它的导数、积分&#xff0c;甚至微分方程的解。…
暂无图片
编程学习 ·

在职阿里5年,一个28岁女软测工程师的心声

简单的先说一下&#xff0c;坐标杭州&#xff0c;14届本科毕业&#xff0c;算上年前在阿里巴巴的面试&#xff0c;一共有面试了有6家公司&#xff08;因为不想请假&#xff0c;因此只是每个晚上去其他公司面试&#xff0c;所以面试的公司比较少&#xff09; ​ 编辑切换为居中…
暂无图片
编程学习 ·

字符串左旋c语言

目录 题目&#xff1a; 解题思路&#xff1a; 第一步&#xff1a; 第二步&#xff1a; 第三步&#xff1a; 总代码&#xff1a; 题目&#xff1a; 实现一个函数&#xff0c;可以左旋字符串中的k个字符。 例如&#xff1a; ABCD左旋一个字符得到BCDA ABCD左旋两个字符…
暂无图片
编程学习 ·

设计模式--观察者模式笔记

模式的定义与特点 观察者&#xff08;Observer&#xff09;模式的定义&#xff1a;指多个对象间存在一对多的依赖关系&#xff0c;当一个对象的状态发生改变时&#xff0c;所有依赖于它的对象都得到通知并被自动更新。这种模式有时又称作发布-订阅模式、模型-视图模式&#xf…
暂无图片
编程学习 ·

睡觉突然身体动不了,什么是睡眠痽痪症

很多朋友可能有这样的体验&#xff0c;睡觉过程中突然意识清醒&#xff0c;身体却动弹不了。这时候感觉非常恐怖&#xff0c;希望旁边有一个人推自己一下。阳光以前也经常会碰到这样的情况&#xff0c;一年有一百多次&#xff0c;那时候很害怕晚上到来&#xff0c;睡觉了就会出…
暂无图片
编程学习 ·

深入理解C++智能指针——浅析MSVC源码

文章目录unique_ptrshared_ptr 与 weak_ptrstd::bad_weak_ptr 异常std::enable_shared_from_thisunique_ptr unique_ptr 是一个只移型别&#xff08;move-only type&#xff0c;只移型别还有std::mutex等&#xff09;。 结合一下工厂模式&#xff0c;看看其基本用法&#xff…
暂无图片
编程学习 ·

@TableField(exist = false)

TableField(exist false) //申明此字段不在数据库存在&#xff0c;但代码中需要用到它&#xff0c;通知Mybatis-plus在做写库操作是忽略它。,.
暂无图片
编程学习 ·

Java Web day15

第十二章文件上传和下载 一、如何实现文件上传 要实现Web开发中的文件上传功能&#xff0c;通常需要完成两步操作&#xff1a;一.是在Web页面中添加上传输入项&#xff1b;二是在Servlet中读取上传文件的数据&#xff0c;并保存到本地硬盘中。 需要使用一个Apache组织提供一个…
暂无图片
编程学习 ·

【51nod 2478】【单调栈】【前缀和】小b接水

小b接水题目解题思路Code51nod 2478 小b接水 题目 输入样例 12 0 1 0 2 1 0 1 3 2 1 2 1输出样例 6解题思路 可以发现最后能拦住水的都是向两边递减高度&#xff08;&#xff1f;&#xff09; 不管两个高积木之间的的积木是怎样乱七八糟的高度&#xff0c;最后能用来装水的…
暂无图片
编程学习 ·

花了大半天写了一个UVC扩展单元调试工具

基于DIRECTSHOW 实现的&#xff0c;用的是MFC VS2019. 详见&#xff1a;http://www.usbzh.com/article/detail-761.html 获取方法 加QQ群:952873936&#xff0c;然后在群文件\USB调试工具&测试软件\UVCXU-V1.0(UVC扩展单元调试工具-USB中文网官方版).exe USB中文网 USB中文…
暂无图片
编程学习 ·

贪心(一):区间问题、Huffman树

区间问题 例题一&#xff1a;区间选点 给定 N 个闭区间 [ai,bi]请你在数轴上选择尽量少的点&#xff0c;使得每个区间内至少包含一个选出的点。 输出选择的点的最小数量。 位于区间端点上的点也算作区间内。 输入格式 第一行包含整数 N&#xff0c;表示区间数。 接下来 …
暂无图片
编程学习 ·

C语言练习实例——费氏数列

目录 题目 解法 输出结果 题目 Fibonacci为1200年代的欧洲数学家&#xff0c;在他的着作中曾经提到&#xff1a;「若有一只免子每个月生一只小免子&#xff0c;一个月后小免子也开始生产。起初只有一只免子&#xff0c;一个月后就有两只免子&#xff0c;二个月后有三只免子…
暂无图片
编程学习 ·

Android开发(2): Android 资源

个人笔记整理 Android 资源 Android中的资源&#xff0c;一般分为两类&#xff1a; 系统内置资源&#xff1a;Android SDK中所提供的已经定义好的资源&#xff0c;用户可以直接拿来使用。 用户自定义资源&#xff1a;用户自己定义或引入的&#xff0c;只适用于当前应用的资源…
暂无图片
编程学习 ·

零基础如何在短时间内拿到算法offer

​算法工程师是利用算法处理事物的职业 算法&#xff08;Algorithm&#xff09;是一系列解决问题的清晰指令&#xff0c;也就是说&#xff0c;能够对一定规范的输入&#xff0c;在有限时间内获得所要求的输出。 如果一个算法有缺陷&#xff0c;或不适合于某个问题&#xff0c;执…
暂无图片
编程学习 ·

人工智能:知识图谱实战总结

人工智能python&#xff0c;NLP&#xff0c;知识图谱&#xff0c;机器学习&#xff0c;深度学习人工智能&#xff1a;知识图谱实战前言一、实体建模工具Protegepython&#xff0c;NLP&#xff0c;知识图谱&#xff0c;机器学习&#xff0c;深度学习 人工智能&#xff1a;知识图…
暂无图片
编程学习 ·

【无标题】

这里写自定义目录标题欢迎使用Markdown编辑器新的改变功能快捷键合理的创建标题&#xff0c;有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants创建一个自定义列表如何创建一个注…