视觉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. 缺点
视觉里程计能够估算局部时间内的相机运动以及特征点的位置
但是这种局部的方式有明显的缺点：
- 容易丢失
一旦丢失，要么“等待相机转回来”（保存参考帧并与新的帧比较），要么重置整个 VO 以跟踪新的图像数据 - 轨迹漂移
主要原因是每次估计的误差会累计至下一次估计，导致长时间轨迹不准确
大一点儿的局部地图可以缓解这种现象，但它始终是存在的
值得一提的是，如果只关心短时间内的运动，或者 VO 的精度已经满足应用需求
那么有时候可能需要的仅仅就是一个视觉里程计，而不用完全的 SLAM
比如某些无人机控制或 AR 游戏的应用中，用户并不需要一个全局一致的地图
那么轻便的 VO 可能是更好的选择
参考：
相关推荐：
视觉SLAM笔记（47） 优化 PnP 的结果
视觉SLAM笔记（46） 基本的 VO
视觉SLAM笔记（45） 搭建 VO 框架
视觉SLAM笔记（44） RGB-D 的直接法
视觉SLAM笔记（43） 直接法
