一、框架
看一下文件夹组织：
1、bin用来存放可执行的二进制文件。0.1版本主要是搭建SLAM库框架
2、include/myslam存放SLAM模块的头文件，主要是.h文件，声明即可。
3、src存放源代码文件，主要是.cpp文件。
4、test文件是测试SLAM库的文件，也是.cpp文件。
5、lib存放编译好的库文件 .a .so
6、config存放配置文件，也就是需要经常修改的运行参数。这里存放在default.yaml中
7、cmake_modules第三方库的cmake文件，在使用g2o之类的库时会用到。
二、数据结构类
Frame:关键帧，相机位姿估计可以用关键帧描述。
MapPoint:特征点。通常，会把特征点放在地图中，并将新来的帧与地图中特征点进行匹配，完成位姿估计。
Map:管理特征点，添加/删除特征点。
Camera:相机参数，内参、外参以及世界、相机、像素坐标系转换。
Config:参数文件的读取。
1、Camera类：
存储相机的内参K和外参T，有三坐标系（世界坐标系、相机坐标系、像素坐标系）下点的坐标转换函数。
头文件.h
#ifndef CAMERA_H
#define CAMERA_H
#include "myslam/common_include.h"//一些常用的include
namespace myslam
{
//相机类
class Camera
{
public:
//1. 声明变量，相机智能指针、内参、尺度
typedef std::shared_ptr<Camera> Ptr;
float fx_, fy_, cx_, cy_, depth_scale_;
//2. 构造函数
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 )
{}
//3. 世界、相机、像素坐标系转换
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 .cpp执行文件
#include "myslam/camera.h"
namespace myslam
{
//定义构造函数Camera类下
Camera::Camera(){}
//1. 相机和世界转换
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;
}
//2. 相机和像素转化
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
);
}
// 3. 世界和像素转换
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 );
}
}
- 宏定义,防止头文件重复定义
#ifndef CAMERA_H
#define CAMERA_H
{}
#endif
- 命名空间namespace myslam，把类包起来避免定义出同名函数。
- 像素坐标恢复相机和世界坐标需要相机深度depth。
2、MapPoint类
路标点，1、估计它的世界坐标。2、拿当前帧提取的特征点与地图中路标点匹配，进而估计相机运动。
3、记录点被匹配和观测次数，评价好坏程度。
.h
#ifndef MAPPOINT_H
#define MAPPOINT_H
namespace myslam
{
class Frame;
class MapPoint
{
public:
//智能指针，之后使用MapPoint::Ptr即可
typedef shared_ptr<MapPoint> Ptr;
//ID、3D坐标、描述子、被观测次数、正确匹配次数
unsigned long id_;
Vector3d pos_;
Vector3d norm_; // Normal of viewing direction
Mat descriptor_;
int observed_times_;
int correct_times_;
//1. 默认构造函数和初始化构造函数
MapPoint();
MapPoint(long id, Vector3d position, Vector3d norm);
//2. 声明成员函数
static MapPoint::Ptr createMapPoint();
};
}
#endif // MAPPOINT_H.cpp
#include "myslam/common_include.h"
#include "myslam/mappoint.h"
namespace myslam
{
//1. 定义默认构造函数
MapPoint::MapPoint()
: id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), observed_times_(0), correct_times_(0){}
//2. 定义初始化构造函数
MapPoint::MapPoint ( long id, Vector3d position, Vector3d norm )
: id_(id), pos_(position), norm_(norm), observed_times_(0), correct_times_(0){}
//3. 定义成员函数
MapPoint::Ptr MapPoint::createMapPoint()
{
static long factory_id = 0;
return MapPoint::Ptr( new MapPoint( factory_id++, Vector3d(0,0,0), Vector3d(0,0,0) ));
}
}- 智能指针定义成MapPoint的指针类型 typedef shared_ptr<MapPoint> Ptr; MapPoint::Ptr
//声明成员函数
static MapPoint::Ptr createMapPoint();
//定义成员函数
MapPoint::Ptr MapPoint::createMapPoint()
{
static long factory_id = 0;
return MapPoint::Ptr( new MapPoint( factory_id++, Vector3d(0,0,0), Vector3d(0,0,0) ));
}
3、Map类
Map管理路标点，增添、删除路标点。VO 匹配过程只和Map类打交道。
随机访问，随时插入删除，使用哈希存储。
unordered_map<unsigned long, MapPoint::Ptr > map_points_; .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_; // all landmarks
unordered_map<unsigned long, Frame::Ptr > keyframes_; // all key-frames
//默认构造函数
Map() {}
//成员函数，添加关键帧、路标点
void insertKeyFrame( Frame::Ptr frame );
void insertMapPoint( MapPoint::Ptr map_point );
};
}
#endif // MAP_H.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;
}
}
}4、Frame类
定义了ID、时间戳、位姿、相机、图像
函数：创建Frame、寻找点深度、获取相机光心、判断点是否在视野内。
.h
#ifndef FRAME_H
#define FRAME_H
#include "myslam/common_include.h"
#include "myslam/camera.h"
namespace myslam
{
class MapPoint;
class Frame
{
public:
//ID、时间戳、位姿、相机、图像
typedef std::shared_ptr<Frame> Ptr;
unsigned long id_; // id of this frame
double time_stamp_; // when it is recorded
SE3 T_c_w_; // transform from world to camera
Camera::Ptr camera_; // Pinhole RGBD Camera model
Mat color_, depth_; // color and depth image
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();
//1. 成员函数，创建Frame
static Frame::Ptr createFrame();
//2. 寻找点的深度
double findDepth( const cv::KeyPoint& kp );
//3. 获取相机光心
Vector3d getCamCenter() const;
//4. 检查是否点在视野内
bool isInFrame( const Vector3d& pt_world );
};
}
#endif // FRAME_H.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(){}
//1. 定义成员函数，创建Frame
Frame::Ptr Frame::createFrame()
{
static long factory_id = 0;
return Frame::Ptr( new Frame(factory_id++) );
}
//2. 寻找关键点的深度
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];//.ptr模板函数定位像素值的方法
if ( d!=0 )
{
return double(d)/camera_->depth_scale_;
}
else
{
//如果该点深度为0，检查周围四个点是否有深度值
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;
}
//3. 获取相机光心
Vector3d Frame::getCamCenter() const
{
return T_c_w_.inverse().translation();//.translation()是取平移部分
}
bool Frame::isInFrame ( const Vector3d& pt_world )
{
Vector3d p_cam = camera_->world2camera( pt_world, T_c_w_ );
//z小于0错误
if ( p_cam(2,0)<0 )
return false;
Vector2d pixel = camera_->world2pixel( pt_world, T_c_w_ );
//xy都大于0且小于图像行列
return pixel(0,0)>0 && pixel(1,0)>0
&& pixel(0,0)<color_.cols
&& pixel(1,0)<color_.rows;
}
}5、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_;//文件读取，FileStorage类可以访问任意字段
Config () {}
public:
~Config();
//1. 成员函数，设置参数
static void setParameterFile( const std::string& filename );
//2. get函数，用到了file_[key]
template< typename T >
static T get( const std::string& key )
{
return T( Config::config_->file_[key] );
}
};
}
#endif // CONFIG_H.cpp
#include "myslam/config.h"
namespace myslam
{
//1. 定义成员函数，设置参数
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;
}
}
//2. 定义析构函数
Config::~Config()
{
if ( file_.isOpened() )
file_.release();
}
shared_ptr<Config> Config::config_ = nullptr;
}- 实现中，只需要判断参数文件是否存在。
读取数据
myslam::Config::setParameterFile("parameter.yaml");
double fx=myslam::Config::get<double> ("Camera.fx");