视觉SLAM笔记(48) 局部地图


1. 迭代优化的问题

引入迭代优化方法之后,估计结果的质量比纯粹 RANSAC PnP 有明显的提高
尽管依然仅使用两两帧间的信息,但得到的运动却更加准确、平稳
从改进中,看到了优化的重要性
不过, 0 VO 仍受两两帧间匹配的局限性影响
一旦视频序列当中某个帧丢失,就会导致后续的帧也无法和上一帧匹配
下面,把地图引入到 VO 中来


2. 特征点引入地图

将 VO 匹配到的特征点放到地图中,并将当前帧与地图点进行匹配,计算位姿
这种做法与之前的差异可见图 :
在这里插入图片描述

在两两帧间比较时,只计算参考帧与当前帧之间的特征匹配和运动关系,在计算之后把当前帧设为新的参考帧
而在使用地图的 VO 中,每个帧为地图贡献一些信息,比方说添加新的特征点或更新旧特征点的位置计
地图中的特征点位置往往是使用世界坐标的

因此,当前帧到来时,我们求它和地图之间的特征匹配与运动关系,即直接计算了Tcw

这样做的好处是,能够维护一个不断更新的地图
只要地图是正确的,即使中间某帧出了差错,仍有希望求出之后那些帧的正确位置
请注意,现在还没有详细地讨论 SLAM 的建图问题
所以这里的地图仅是一个临时性的概念,指的是把各帧特征点缓存到一个地方,构成了特征点的集合,称它为地图


2. 地图

地图又可以分为局部(Local)地图和全局(Global)地图两种,由于用途不同,往往分开讨论

顾名思义,局部地图描述了附近的特征点信息:
只保留离相机当前位置较近的特征点,而把远的或视野外的特征点丢掉
这些特征点是用来和当前帧匹配来求相机位置的,所以希望它能够做的比较快

另一方面,全局地图则记录了从 SLAM 运行以来的所有特征点
它显然规模要大一些,主要用来表达整个环境
但是直接在全局地图上定位,对计算机的负担就太大了
它主要用于回环检测地图表达

在视觉里程计中,更关心可以直接用于定位的局部地图(如果决心要用地图的话)
随着相机运动,往地图里添加新的特征点,并去掉之前
注意的是:是否使用地图取决对精度——效率这个矛盾的把握

完全可以出于效率的考量,使用两两无结构式的 VO
也可以为了更好的精度,构建局部地图乃至考虑地图的优化

局部地图的一件麻烦事是维护它的规模
为了保证实时性,需要保证地图规模不至于太大(否则匹配会消耗大量的时间)
此外,单个帧与地图的特征匹配存在着一些加速手段
但由于它们技术上比较复杂,这里就不给出了


3. 地图点类

现在,来实现地图点类
稍加完善之前没有用到的 MapPoint 类,位于/include/myslam/mappoint.h
主要是它的构造函数和生成函数

#ifndef MAPPOINT_H
#define MAPPOINT_H

#include "myslam/common_include.h"

namespace myslam
{
    class Frame;
    class MapPoint
    {
    public:
        typedef shared_ptr<MapPoint> Ptr;
        unsigned long      id_;           // ID
        static unsigned long factory_id_;    
        bool        good_;                // 是否是好点
        Vector3d    pos_;                 // 世界坐标系上的坐标
        Vector3d    norm_;                // 观察方向法线
        Mat         descriptor_;          // 描述符匹配

        list<Frame*>    observed_frames_; // 观察时间

        int         matched_times_;       // 匹配时间
        int         visible_times_;       // 一帧时间

        MapPoint();
        MapPoint(
            unsigned long id,
            const Vector3d& position,
            const Vector3d& norm,
            Frame* frame = nullptr,
            const Mat& descriptor = Mat()
        );

        inline cv::Point3f getPositionCV() const {
            return cv::Point3f(pos_(0, 0), pos_(1, 0), pos_(2, 0));
        }

        // 建立MapPoint
        static MapPoint::Ptr createMapPoint();
        static MapPoint::Ptr createMapPoint(
            const Vector3d& pos_world,
            const Vector3d& norm_,
            const Mat& descriptor,
            Frame* frame);
    };
}

#endif // MAPPOINT_H

3. 视觉里程类

主要的修改在 VisualOdometry 类上
由于工作流程的改变,修改了它的几个主要函数
例如每次循环中要对地图进行增删、统计每个地图点被观测到的次数等等


3.1. 关键帧

在提取第一帧的特征点之后,将第一帧的所有特征点全部放入地图中

void VisualOdometry::addKeyFrame()
{
    if (map_->keyframes_.empty())
    {
        // 第一个关键帧,添加所有3d点到地图
        for (size_t i = 0; i < keypoints_curr_.size(); i++)
        {
            double d = curr_->findDepth(keypoints_curr_[i]);
            if (d < 0)
                continue;
            Vector3d p_world = ref_->camera_->pixel2world(
                Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), curr_->T_c_w_, d
            );
            Vector3d n = p_world - ref_->getCamCenter();
            n.normalize();
            MapPoint::Ptr map_point = MapPoint::createMapPoint(
                p_world, n, descriptors_curr_.row(i).clone(), curr_.get()
            );
            map_->insertMapPoint(map_point);
        }
    }

    map_->insertKeyFrame(curr_);
    ref_ = curr_;
}

除了现有的地图之外,还引入了“关键帧”(Key-frame)的概念
关键帧在许多视觉 SLAM 中都会用到,不过这个概念主要是给后端用的
在实践中,肯定不希望对每个图像都做详细的优化和回环检测,那样毕竟太耗费资源
至少相机搁在原地不动时,不希望整个模型(地图也好、轨迹也好)变得越来越大
因此,后端优化的主要对象就是关键帧

关键帧是相机运动过程当中某几个特殊的帧,这里“特殊”的意义是可以由自己指定的
常见的做法时,每当相机运动经过一定间隔,就取一个新的关键帧并保存起来
这些关键帧的位姿将被仔细优化
而位于两个关键帧之间的那些东西,除了对地图贡献一些地图点外,就被理所当然地忽略掉了


3.2. 优化地图

后续的帧中,使用 OptimizeMap 函数对地图进行优化
包括删除不在视野内的点,在匹配数量减少时添加新点等等

void VisualOdometry::optimizeMap()
{
    // 除去几乎看不见的和不可见的点
    for (auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
    {
        if (!curr_->isInFrame(iter->second->pos_))
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        float match_ratio = float(iter->second->matched_times_) / iter->second->visible_times_;
        if (match_ratio < map_point_erase_ratio_)
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }

        double angle = getViewAngle(curr_, iter->second);
        if (angle > M_PI / 6.)
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        if (iter->second->good_ == false)
        {
            // 试着对这个地图点进行三角测量
        }
        iter++;
    }

    if (match_2dkp_index_.size() < 100)
        addMapPoints();
    if (map_->map_points_.size() > 1000)
    {
        // 地图太大,删除一些
        map_point_erase_ratio_ += 0.05;
    }
    else
        map_point_erase_ratio_ = 0.1;
    cout << "map points: " << map_->map_points_.size() << endl;
}

3.3. 特征匹配

匹配之前,从地图中拿出一些候选点(出现在视野内的点)
然后将它们与当前帧的特征描述子进行匹配

void VisualOdometry::featureMatching()
{
    boost::timer timer;
    vector<cv::DMatch> matches;
    // 在map中选择候选项
    Mat desp_map;
    vector<MapPoint::Ptr> candidate;
    for (auto& allpoints : map_->map_points_)
    {
        MapPoint::Ptr& p = allpoints.second;
        // 检查p是否在当前帧图像中
        if (curr_->isInFrame(p->pos_))
        {
            // 添加到候选
            p->visible_times_++;
            candidate.push_back(p);
            desp_map.push_back(p->descriptor_);
        }
    }

    matcher_flann_.match(desp_map, descriptors_curr_, matches);
    // 选择最佳匹配
    float min_dis = std::min_element(
        matches.begin(), matches.end(),
        [](const cv::DMatch& m1, const cv::DMatch& m2)
    {
        return m1.distance < m2.distance;
    })->distance;

    match_3dpts_.clear();
    match_2dkp_index_.clear();
    for (cv::DMatch& m : matches)
    {
        if (m.distance < max<float>(min_dis*match_ratio_, 30.0))
        {
            match_3dpts_.push_back(candidate[m.queryIdx]);
            match_2dkp_index_.push_back(m.trainIdx);
        }
    }
    cout << "good matches: " << match_3dpts_.size() << endl;
    cout << "match cost time: " << timer.elapsed() << endl;
}

4. 结果对比

运行此程序
在这里插入图片描述
如果位姿估计正确的话,它们看起来应该像是固定在空间中一样
如果感觉到某个特征点不自然地运动,那可能是相机位姿估计不够准确,或特征点的位置不够准确

在这里插入图片描述


5. 缺点

视觉里程计能够估算局部时间内的相机运动以及特征点的位置
但是这种局部的方式有明显的缺点:

  1. 容易丢失
    一旦丢失,要么“等待相机转回来”(保存参考帧并与新的帧比较),要么重置整个 VO 以跟踪新的图像数据
  2. 轨迹漂移
    主要原因是每次估计的误差会累计至下一次估计,导致长时间轨迹不准确
    大一点儿的局部地图可以缓解这种现象,但它始终是存在的

值得一提的是,如果只关心短时间内的运动,或者 VO 的精度已经满足应用需求
那么有时候可能需要的仅仅就是一个视觉里程计,而不用完全的 SLAM
比如某些无人机控制或 AR 游戏的应用中,用户并不需要一个全局一致的地图
那么轻便的 VO 可能是更好的选择


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(47) 优化 PnP 的结果
视觉SLAM笔记(46) 基本的 VO
视觉SLAM笔记(45) 搭建 VO 框架
视觉SLAM笔记(44) RGB-D 的直接法
视觉SLAM笔记(43) 直接法


发布了265 篇原创文章 · 获赞 358 · 访问量 337万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览