1.搭建VO框架
A. SLAM库是一个小型库,库的组织结构如下:
1.bin 用来存放可执行的二进制文件。
2.include/myslam存放SLAM模块的头文件,主要是.h文件,当把包含目录设到include,引用自己的头文件时,需要写include“myslam/xxx.h”。
3.src 存放源代码文件,主要是.cpp文件。
4.test 存放测试文件,也是.cpp文件。
5.lib 存放编译好的库文件。
6.config 存放配置文件。
7.cmake_modules 第三方库的cmake文件,在使用g2o之类的库会用到它。
2 . 确定基本数据结构
1.帧:相机采集到的图像单位,包含一个或一对图像。还有特征点、位姿、内参等信息。
2.路标:路标即图像中的特征点,在相机运动后,我们还能估计他们的3D位置(对极几何、PnP、ICP、光流法和直接法等)。通常都是把路标点放在一个地图当中,并将新来的帧与地图中的路标点进行匹配,估计相机位姿。
3.配置文件:在程序中遇到各种各样的参数,相机的内参、特征点数量等,较好的方式是在外部定义一个配置文件,程序运行读取该配置文件的参数值。
4.坐标变换:会经常遇到坐标系之间的转换,世界到相机、相机到归一化、归一化到像素等,定义一个类把这些操作都放一起更方便。
3. 5个类及其功能
5个类:Frame为帧,Camera为相机模型、MapPoint为特征点/路标点,Map管理特征点,Config提供配置参数。
camera类:存储摄像机各个坐标系的转换关系
camera.h :
#ifndef CAMERA_H //防止头文件重复引用,防止在俩处引用此头文件时出现类的重复定义
#define CAMERA_H
#include "myslam/common_include.h" //将一些常用的头文件都放在这里面,避免每次书写都用很长的include
namespace myslam //用namesapce myslam 将类定义包裹起来
{
// Pinhole RGBD camera model
class Camera
{
public:
typedef std::shared_ptr<Camera> Ptr; //把智能指针定义为camera类型指针,传递函数使用Camera::Ptr即可
float fx_, fy_, cx_, cy_, depth_scale_; // Camera intrinsics
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 )
{}
// coordinate transform: world, camera, pixel vetor3d 三维向量
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
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; //相机坐标系的点转到世界坐标系,T_c_w.inverse()表示逆矩阵
}
Vector2d Camera::camera2pixel ( const Vector3d& p_c )//相机空间点转化成像素点
{
return Vector2d (
fx_ * p_c ( 0