视觉SLAM笔记（45） 搭建 VO 框架
视觉SLAM框架的构建涉及多个关键组件的设计与实现。该框架以程序框架为核心，主要包括Frame、MapPoint、Map及Camera等主要数据结构。Frame类负责存储帧信息并提供坐标变换功能；MapPoint类管理路标点及其描述符；Map类整合所有路标点与关键帧；Camera类则定义相机模型并完成内外参计算。通过这些组件的协同工作，实现了从RGB-D到双目等多模态SLAM算法的基本功能。同时，配置文件用于参数化设置，默认值为合理 defaults，并通过共享指针进行管理以避免重复初始化问题。整体设计遵循模块化原则与代码规范性要求。
视觉SLAM笔记（45） 搭建 VO 框架
- 1. 规划程序架构
- 2. 明确数据模型
- 3. 相机类
- 4. 帧类
- 5. 坐标点类
- 6. 地图类
- 7. 配置类
1. 确定程序框架
基于过往的信息可知视觉里程计分为单目相机、双目摄像头以及RGB-D三类。相比之下,单目视觉较为复杂,而RGB-D相对而言最为简单,无需初始化步骤,同样无需面对尺度缩放的问题
循序渐进地开展研究工作，在RGB-D技术应用的基础上起步
为了简化实验过程，在实验阶段采用模拟数据集替代真实RGB-D相机设备
在开发小型库项目时，常见做法是合理设置存储结构。具体来说，在源代码管理方面采用模块化设计有助于提升开发效率；将头文件与源代码分隔存档有助于提高可维护性；而对文档和测试数据等重要资源进行专门目录分配，则能显著提升项目的可读性和扩展性
如果一个库内容丰富，并且会将代码划分为若干个独立的小模块。便于实现高效的测试流程。
参考 OpenCV 和 g2o 的组织架构模式。
例如, OpenCV 被划分为核心模块（core）、图像处理模块（imgproc）以及特征提取模块（features2d）等主要部分。
g2o 则被划分为核心组件（core）、求解器组件（solvers）以及类型组件（types）等主要部分。
即使是小型程序,也可以将各种功能整合在一起形成一个 SLAM 库。
目前致力于编写一个小型SLAM库,其主要目标在于整合先前开发的各种算法,并撰写属于自己的SLAM程序.在项目初期,建议选择一个工程根目录,随后按照需求创建特定的文件夹结构以合理组织代码资源
-
bin用于存储可执行的二进制程序。 -
include/myslam存放 slam 模块相关的头文件，并采用 .h 扩展名。
这种做法的原因在于：当将包含目录设置为 include 时，在引用本模块的头文件时 需要用include "myslam/xxx.h"这种方式以避免与其他库文件产生混淆。
其主要原因是 当将包含目录设置为 include 时，在引用本模块的头文件时 需要用include "myslam/xxx.h"这种方式以避免与其他库文件产生混淆。
这种方法的设计初衷是为了减少与外部库文件之间的潜在冲突。 -
src用于存储源代码文件, 主要是 cpp 类型。 -
test用作测试相关文件存储, 同样是 cpp 类型。 -
lib包含预先编译好的库文件。 -
config配置信息以配置文件形式存在。 -
cmake_modules是第三方库的 cmake 文件, 在使用像 g2o 这样的库时会将其包含进来。
以上就是目录结构，如图所示
相较于过去在各章节中分散放置的 main.cpp，
这种安排更加井然有序。
随后，在各个目录中逐步增加新的代码文件，
最终构建起一个完整的项目结构。
2. 确定基本数据结构
为了使程序正常运行，在构建完善的数学模型并进行系统优化的基础上完成相关的算法开发
帧 ：一个帧是相机采集到的图像单位
它主要包含一个图像（RGB-D 情形下是一对图像），还有特征点、位姿、内参等信息
在视觉 SLAM 中会谈论关键帧（Key-frame）
由于相机采集的数据很多，存储所有的数据显然是不现实的
通常的做法是把某些认为更重要的帧保存起来，并认为相机轨迹就可以用这些关键帧来描述
关键帧如何选择是一个很大的问题，而且基于工程经验，很少有理论上的指导
这里也会使用一个关键帧选择方法
路标：路标点即图像中的关键特征点
相机运动后可推算出其三维位置
通常会将路标点放置于地图中，并利用新帧与地图中的路标点进行配准以推算相机位姿
该问题等同于解决一个局部的 SLAM 问题。
然而，在实现这一目标的过程中，
还需引入一些辅助工具以提升程序运行效率。
这些工具不仅能够优化算法性能，
还能有效减少计算复杂度。
配置文件 ：在写程序中会经常遇到各种各样的参数
比如相机的内参、特征点的数量、匹配时选择的比例等等
可以把这些数写在程序中，但那不是一个好习惯
会经常修改这些参数，但每次修改后都要重新编译一遍程序
当它们数量越来越多时，修改就变得越来越困难
所以，更好的方式是在 外部定义一个配置文件 ，程序运行时读取该配置文件中的参数值
这样，每次只要修改配置文件内容就行了，不必对程序本身做任何修改
在不同坐標體之間進行坐標轉換是一项常見的需求。例如，在從 世 �界坐標體轉換至相機坐標體、随后從相機坐標體轉換至歸一化相機坐標體、最後進一步將其轉換至像素坐標體等環節中可见常出现的情況。為提高效率與可重用性, 值得考慮创建一個類來整合這些操作步驟,這樣會更加便捷。
首先定义帧、标志等基本概念，在 C++ 中每个类都独立成一个头文件。建议每个类独立拥有头文件和源 files 以避免混杂。接下来将函数 declarations 安排在 head files 中 while implementations 则位于 source files 中（除非函数极为简短，则可考虑将其包含在 head files 内）。遵循 Google 开源项目的命名规范，并且考虑到代码主要关注算法实现而非复杂 software 工程问题无需深入讨论复杂的继承关系、接口或模板等高级概念。同时要确保代码易于理解并具备扩展性——会把数据成员设置为公有属性以确保代码可见性和可维护性。
对于较为复杂的算法而言，在设计时通常会将其拆解为多个步骤来处理。例如，在具体实施过程中应当各自独立地实现到不同的函数模块中。这样一来，在对算法流程进行调整时就不会影响整体运行流程只需专注于局部模块的优化即可。
现在，开始写 VO，这是刚开始的阶段
一共写五个类：
帧块
帧块
它们的关系如图所示：
目前仅需列举其数据成员及常用方法，在后续涉及相关内容时再行补充。
3. Camera 类
从最基础的Camera类开始实现。
首先记录相机的内参与外参参数，并完成相机坐标系与像素坐标系、世界坐标系间的转换关系。
在世界坐标系中，则需要通过参数化的方式获取相机的外参数据。
在源文件中，Camera类的头文件位于 /include/myslam/camera.h：
#ifndef CAMERA_H // 防止头文件重复引用
#define CAMERA_H
#include "myslam/common_include.h"
namespace myslam
{
// 针孔/RGBD 相机模型
class Camera
{
public:
typedef std::shared_ptr<Camera> Ptr;
float fx_, fy_, cx_, cy_, depth_scale_; // 内参
Camera(); // 定义 Camera 的指针类型
Camera(float fx, float fy, float cx, float cy, float depth_scale = 0) :
fx_(fx), fy_(fy), cx_(cx), cy_(cy), depth_scale_(depth_scale)
{}
// 坐标变换:世界，相机，像素
Vector3d world2camera(const Vector3d& p_w, const SE3& T_c_w);
Vector3d camera2world(const Vector3d& p_c, const SE3& T_c_w);
Vector2d camera2pixel(const Vector3d& p_c);
Vector3d pixel2camera(const Vector2d& p_p, double depth = 1);
Vector3d pixel2world(const Vector2d& p_p, const SE3& T_c_w, double depth = 1);
Vector2d world2pixel(const Vector3d& p_w, const SE3& T_c_w);
};
}
#endif // CAMERA_H
AI助手将类定义包裹于名为 myslam 的命名空间内。由于本项目直接实现 SLAM 逻辑，因此选择 myslam 作为内建命名空间更为合适。通过使用命名空间机制不仅可以避免因误名导致的功能冲突，也能遵循更规范的编程实践。
由于宏定义和命名空间在每个文件中都会写一遍
把一些常用的头文件放在一个 common_include.h 文件中
这样就可以避免每次书写一个很长的一串 include
将智能指针归类为Camera的指针类型。
这样，在传递参数时，就可以使用Camera::Ptr来传递参数。
用 Sophus::SE3 来表达相机的位姿
在源文件中，给出 Camera 方法的实现 /src/camera.cpp ：
#include "myslam/camera.h"
namespace myslam
{
Camera::Camera()
{
}
Vector3d Camera::world2camera(const Vector3d& p_w, const SE3& T_c_w)
{
return T_c_w * p_w;
}
Vector3d Camera::camera2world(const Vector3d& p_c, const SE3& T_c_w)
{
return T_c_w.inverse() *p_c;
}
Vector2d Camera::camera2pixel(const Vector3d& p_c)
{
return Vector2d(
fx_ * p_c(0, 0) / p_c(2, 0) + cx_,
fy_ * p_c(1, 0) / p_c(2, 0) + cy_
);
}
Vector3d Camera::pixel2camera(const Vector2d& p_p, double depth)
{
return Vector3d(
(p_p(0, 0) - cx_) *depth / fx_,
(p_p(1, 0) - cy_) *depth / fy_,
depth
);
}
Vector2d Camera::world2pixel(const Vector3d& p_w, const SE3& T_c_w)
{
return camera2pixel(world2camera(p_w, T_c_w));
}
Vector3d Camera::pixel2world(const Vector2d& p_p, const SE3& T_c_w, double depth)
{
return camera2world(pixel2camera(p_p, depth), T_c_w);
}
}
AI助手完成了像素坐标系、相机坐标系和世界坐标系间的坐标变换
4. Frame 类
在软件架构中，Frame 类作为基础数据单元，在多个地方都会被使用。然而，在初期设计阶段尚未明确未来可能会增加的新内容。因此，在当前框架中仅包含基础的数据存储和接口功能。如果后续如果有新增内容，则会逐步纳入其中。
在源文件中，Frame 类的头文件位于 /include/myslam/frame.h ：
#ifndef FRAME_H
#define FRAME_H
#include "myslam/common_include.h"
#include "myslam/camera.h"
namespace myslam
{
class MapPoint;
class Frame
{
public:
typedef std::shared_ptr<Frame> Ptr;
unsigned long id_; // 帧的id
double time_stamp_; // 记录的时间
SE3 T_c_w_; // 从世界到相机的转换
Camera::Ptr camera_; // 针孔/RGBD相机模型
Mat color_, depth_; // 颜色和深度图像
public: // 数据成员
Frame();
Frame(long id, double time_stamp = 0, SE3 T_c_w = SE3(), Camera::Ptr camera = nullptr, Mat color = Mat(), Mat depth = Mat());
~Frame();
// 创建 Frame
static Frame::Ptr createFrame();
// 寻找给定点对应的深度
double findDepth(const cv::KeyPoint& kp);
// 获取相机光心
Vector3d getCamCenter() const;
// 判断某个点是否在视野内
bool isInFrame(const Vector3d& pt_world);
};
}
#endif // FRAME_H
AI助手在Frame中包含了关键的参数：ID用于标识该帧，在时间轴上标明其出现的时间戳，在空间中记录位姿信息，并通过相机捕捉图像数据。
该模块实现了以下核心功能：支持创建新的Frame对象(CreateFrame)；根据三维点推算其对应的深度(FindPointDepth)；获取相机中心位置(GetCameraCenter)；判断点是否位于观察范围内(CheckVisibility)。这些操作构成了该模块的基本功能。
在源文件中，给出Frame方法的实现 /src/frame.cpp：
#include "myslam/frame.h"
namespace myslam
{
Frame::Frame()
: id_(-1), time_stamp_(-1), camera_(nullptr)
{
}
Frame::Frame(long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth)
: id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth)
{
}
Frame::~Frame()
{
}
// 创建 Frame
Frame::Ptr Frame::createFrame()
{
static long factory_id = 0;
return Frame::Ptr(new Frame(factory_id++));
}
// 寻找给定点对应的深度
double Frame::findDepth(const cv::KeyPoint& kp)
{
int x = cvRound(kp.pt.x);
int y = cvRound(kp.pt.y);
ushort d = depth_.ptr<ushort>(y)[x];
if (d != 0)
{
return double(d) / camera_->depth_scale_;
}
else
{
// 检查附近的地点
int dx[4] = { -1,0,1,0 };
int dy[4] = { 0,-1,0,1 };
for (int i = 0; i < 4; i++)
{
d = depth_.ptr<ushort>(y + dy[i])[x + dx[i]];
if (d != 0)
{
return double(d) / camera_->depth_scale_;
}
}
}
return -1.0;
}
// 获取相机光心
Vector3d Frame::getCamCenter() const
{
return T_c_w_.inverse().translation();
}
// 判断某个点是否在视野内
bool Frame::isInFrame(const Vector3d& pt_world)
{
Vector3d p_cam = camera_->world2camera(pt_world, T_c_w_);
if (p_cam(2, 0) < 0)
return false;
Vector2d pixel = camera_->world2pixel(pt_world, T_c_w_);
return pixel(0, 0) > 0 && pixel(1, 0) > 0
&& pixel(0, 0) < color_.cols
&& pixel(1, 0) < color_.rows;
}
}
AI助手5. MapPoint 类
在该系统中使用MapPoint表示路标点，并用于估计该路标的三维世界坐标位置。系统会利用当前帧提取出的特征点与地图中的预设路标点进行匹配，并通过此过程来推算相机的姿态变化。为了更好地完成这一任务，在系统运行过程中还需要存储每个特征点与其对应的描述子信息。同时需要统计每个特征点被观测、匹配成功的次数以及整体匹配率等数据。以上改进有助于提高系统的定位精度和鲁棒性。
在源文件中，MapPoint 类的头文件位于 /include/myslam/mappoint.h ：
#ifndef MAPPOINT_H
#define MAPPOINT_H
namespace myslam
{
class Frame;
class MapPoint
{
public:
typedef shared_ptr<MapPoint> Ptr;
unsigned long id_; // ID
Vector3d pos_; // 世界坐标系上的坐标
Vector3d norm_; // 观察方向法线
Mat descriptor_; // 描述符匹配
int observed_times_; // 观察时间
int correct_times_; // 正确时间
MapPoint();
MapPoint(long id, Vector3d position, Vector3d norm);
// 建立MapPoint
static MapPoint::Ptr createMapPoint();
};
}
#endif // MAPPOINT_H
AI助手在源文件中，给出 MapPoint 方法的实现 /src/mappoint.cpp ：
#include "myslam/common_include.h"
#include "myslam/mappoint.h"
namespace myslam
{
MapPoint::MapPoint()
: id_(-1), pos_(Vector3d(0, 0, 0)), norm_(Vector3d(0, 0, 0)), observed_times_(0), correct_times_(0)
{
}
MapPoint::MapPoint(long id, Vector3d position, Vector3d norm)
: id_(id), pos_(position), norm_(norm), observed_times_(0), correct_times_(0)
{
}
// 建立MapPoint
MapPoint::Ptr MapPoint::createMapPoint()
{
static long factory_id = 0;
return MapPoint::Ptr(
new MapPoint(factory_id++, Vector3d(0, 0, 0), Vector3d(0, 0, 0))
);
}
}
AI助手6. Map 类
Map 类作为维护所有路标点的核心实体，在日常运营中不仅负责维护现有路标点的记录信息，还承担着动态更新现有路标的任务。 VO 的匹配过程主要依赖于与 Map 系统之间的交互机制。 在实际应用中，Map 类可能会涉及多种操作功能；然而，在当前阶段我们仅专注于构建核心的数据模型。
在源文件中，Map 类的头文件位于 /include/myslam/map.h ：
#ifndef MAP_H
#define MAP_H
#include "myslam/common_include.h"
#include "myslam/frame.h"
#include "myslam/mappoint.h"
namespace myslam
{
class Map
{
public:
typedef shared_ptr<Map> Ptr;
unordered_map<unsigned long, MapPoint::Ptr > map_points_; // 所有路标点
unordered_map<unsigned long, Frame::Ptr > keyframes_; // 所有关键帧
Map() {}
void insertMapPoint(MapPoint::Ptr map_point); // 插入路标点
void insertKeyFrame(Frame::Ptr frame); // 插入关键帧
};
}
#endif // MAP_H
AI助手Map类中具体存储了各个关键帧和路标点。这些数据不仅需要能够快速访问任意位置的信息，并且还需支持动态增删操作。因此采用哈希表（Hash）结构来进行存储。
在源文件中，给出 Map 方法的实现 /src/map.cpp ：
#include "myslam/map.h"
namespace myslam
{
void Map::insertKeyFrame(Frame::Ptr frame)
{
cout << "Key frame size = " << keyframes_.size() << endl;
if (keyframes_.find(frame->id_) == keyframes_.end())
{
keyframes_.insert(make_pair(frame->id_, frame));
}
else
{
keyframes_[frame->id_] = frame;
}
}
void Map::insertMapPoint(MapPoint::Ptr map_point)
{
if (map_points_.find(map_point->id_) == map_points_.end())
{
map_points_.insert(make_pair(map_point->id_, map_point));
}
else
{
map_points_[map_point->id_] = map_point;
}
}
}
AI助手7. Config 类
该类主要负责从配置文件中读取数据，并在全球范围内提供这些数据的即时访问。
因此将该类设计为单例模式更为合适。
当配置文件被设置时
创建实例并在初始化阶段完成配置数据读取。
之后就可以在全球范围内随时访问这些数据
确保资源得到及时释放。
在源文件中，Config 类的头文件位于 /include/myslam/config.h ：
#ifndef CONFIG_H
#define CONFIG_H
#include "myslam/common_include.h"
namespace myslam
{
class Config
{
private:
static std::shared_ptr<Config> config_;
cv::FileStorage file_;
Config() {}
public:
~Config();
// 设置一个新的配置文件
static void setParameterFile(const std::string& filename);
// 访问参数值
template< typename T >
static T get(const std::string& key)
{
return T(Config::config_->file_[key]);
}
};
}
#endif // CONFIG_H
AI助手将类的构造函数设为私有以避免其被外部代码创建，并仅能在setParameterFile方法中进行构造。
实际生成的对象属于Config的智能指针实现：static shared_ptr<Config>config_
主要原因在于使用智能指针后能够自动完成对象的析构操作而无需额外调用其他函数来完成析构。
在文件读取操作中，OpenCV 提供的 FileStorage 类是一个强大的工具。该类不仅支持读取并解析各种格式的数据文件（如 YAML、JSON 等），还可以灵活地访问和管理存储在这些文件中的各个字段数据。为了适应不同场景的需求，在获取具体数据时可以通过调用 get 操作方法来实现对任意类型数据值的有效获取和处理。
在源文件中，给出 Config 方法的实现 /src/config.cpp ：
#include "myslam/config.h"
namespace myslam
{
void Config::setParameterFile(const std::string& filename)
{
if (config_ == nullptr)
config_ = shared_ptr<Config>(new Config);
config_->file_ = cv::FileStorage(filename.c_str(), cv::FileStorage::READ);
if (config_->file_.isOpened() == false)
{
std::cerr << "parameter file " << filename << " does not exist." << std::endl;
config_->file_.release();
return;
}
}
Config::~Config()
{
if (file_.isOpened())
file_.release();
}
shared_ptr<Config> Config::config_ = nullptr;
}
AI助手在实现过程中,只需判断参数文件是否存在问题.一旦定义了这个Config类,就可以方便地获取参数文件中的参数.
例如，当想要定义相机的焦距 fx 时，按照以下几个操作步骤即可：
- 在参数文件中加入：“Camera.fx: 500”。
- 在代码中使用：
myslam::Config::setParameterFile("parameter.yaml");
double fx = myslam::Config::get<double>("Camera.fx");
AI助手就能获得 fx 的值了
当然，在开发过程中优先考虑提高程序开发效率的同时，在程序开发过程中优先考虑提高程序开发效率的同时，
也可以采用更为简便的方式进行参数配置。
当然也不止如此
至此, 构建了 SLAM 程序的基本数据结构, 并编写了若干个基础类. 通过 cmake 可以进行编译该工程, 然而该程序目前不具备实质的功能.
参考：
相关推荐：
[计算机视觉中的SLAM技术（44） 基于RGB-D模型的直接解算方法]
[计算机视觉中的SLAM技术（43） 直接解算方法研究综述]
[计算机视觉中的SLAM技术（42） 基于光流法的特征点追踪技术]
[计算机视觉中的SLAM技术（41） 光流算法及其应用]
[计算机视觉中的SLAM技术（40） 特征点检测与描述器缺陷分析]