在正式写系统之前，我们在上一篇分析了基本的3个类：帧、2D特征点、3D地图点，这次我们开始代码实现这些基本数据结构：
1.帧类
常见的SLAM系统中的帧（Frame）需要包含以下信息：id，位姿，图像，左右目特征点
frame.h:
在抽象的过程中，我们分为参数和函数的确定，首先是参数：
- 该帧的id
- 该帧作为关键帧（keyframe）的id
- 是否为关键帧
- 时间戳
- TCW类型的位姿（pose）
- pose的数据锁
- 该帧能看到的双目图像
- 该帧能看到的双目特征点
之后是构造函数，以及一些功能函数：
- 无参构造函数
- 有参构造函数，将各个参数初始化（输入id，时间戳，位姿，左右目图像）
由于pose会被前后端多个线程设置或访问，因此需要定义pose的set和get函数，调用函数时还需要加线程锁）：
- set函数：设置帧的位姿，并保证线程安全
- get函数：取出帧的位姿，并保证线程安全
最后通过工厂模式构建Frame，并在静态函数中自动分配id
- 工厂构建函数
// Frame类含有id，位姿，图像，左右目特征点
#pragma once
#ifndef MYSLAM_FRAME_H
#define MYSLAM_FRAME_H
#include"MYSLAM/common_include.h"
namespace MYSLAM {
// 前向声明
struct MapPoint;
struct Feature;
// 开始定义Frame类
struct Frame
{
// 1.定义所需的参数
public :
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // 用于在Eigen C++库中启用对齐内存分配
typedef std::shared_ptr<Frame> Ptr; // 定义了一个shared_ptr类型的指针
unsigned long id_ = 0; // 该帧的id
unsigned long keyframe_id_ = 0; // 该帧作为keyframe的id
bool is_keyframe_ = true; // 是否为关键帧
double time_stamp_; // 时间戳
SE3 pose_; // TCW类型的pose
std::mutex pose_mutex_; // pose的数据锁
Mat left_img_, right_img_; // 该帧能看到的双目图像
// 该帧能看到的双目特征点(定义存放左图、右图特征点指针的容器)
std::vector<std::shared_ptr<Feature>> features_left_;
std::vector<std::shared_ptr<Feature>> features_right;
// 2.定义构造函数和一些成员函数
public:
Frame() {}
// 构造函数，将各个参数初始化（输入id，时间戳，位姿，左右目图像）
Frame(long id, double time_stamp, const SE3 &pose, const Mat &left, const Mat &right);
// 取出帧的位姿，并保证线程安全
SE3 Pose()
{
std::unique_lock<std::mutex> lck(pose_mutex_);//线程锁
return pose_;
}
// 设置帧的位姿，并保证线程安全
void SetPose(const SE3 &pose){
std::unique_lock <std::mutex> lck(pose_mutex_) ;//线程锁
pose_=pose;
}
// 设置关键帧并分配并键帧id
void SetKeyFrame();
// 工厂构建模式，分配id
static std::shared_ptr<Frame>CreateFrame();
};
}
#endif // MYSLAM_FRAME_Hframe.cpp:
#include "MYSLAM/frame.h"
namespace MYSLAM{
//Frame构造函数
Frame::Frame( long id , double time_stamp ,const SE3 &pose, const Mat &left,const Mat &right ):id_(id),time_stamp_(time_stamp), pose_(pose),left_img_(left),right_img_(right) {};
// 设置keyframe的函数
void Frame::SetKeyFrame() {
static long keyframe_factory_id = 0;//关键帧id=0
is_keyframe_ = true; //是否为关键帧置于true
keyframe_id_ = keyframe_factory_id++; //id++
}
//这里注意下，由factory_id++一个数去构造Frame对象时，调用的是默认构造函数，由于默认构造函数全都有默认值，所以就是按坑填，先填第一个id_，
//所以也就是相当于创建了一个只有ID号的空白帧。
Frame::Ptr Frame::CreateFrame(){
static long factory_id =0;
Frame::Ptr new_frame(new Frame);
new_frame->id_=factory_id++;
return new_frame;
}
}//namespace MYSLAM附加知识点：
关于互斥锁：
- std::mutex：书中程序使用的都是std::mutex，与pthread_mutex略有不同，std::mutex是C++语言实现的互斥锁，功能非常简单，具有跨平台的功能。如果对互斥锁没有特别的要求，尽量使用std::mutex。
- unique_lock：unique-lock是一种锁管理模板类，unique-lock对象以独占所有权的方式管理mutex对象的上锁与解锁操作，即：在unique-lock对象的声明周期内，它所管理的锁对象会一直保持上锁的状态，而unique-lock的声明周期结束后，他所管理的对象会被解锁。因此由unqie-lock托管的互斥锁就不必考虑它的解锁操作了。
关于智能指针：
- shared_ptr：智能指针shared ptr部分解决的问题：确保new动态分配的内存空间在程序的各条执行路径都能被释放。将new返回的指针托管给shared-ptr对象托管，就不必担心在哪里写delete了，实际上也不需要自己编写delete，shared-ptr对象消亡时会自动delete该指针，并且shared-ptr对象可以像普通指针一样使用。可以简单理解为能够自动delete的智能指针。
- weak_ptr：weak-ptr用于弥补shared-ptr的计数引用缺陷带来的循环引用问题，weak-ptr本身也是模板类，但不能用于直接定义智能指针，只能配合share-ptr使用，可以将shared-ptr的对象赋值给它，而不增加引用计数，从而避免引用成环（循环引用）的问题。
关于工厂模式：
简单工厂模式：
虚线：表示依赖关系，A指向B，则代表A依赖于B，即A类中使用了B类的属性或方法，但是不改变B的内容。这里指的是工厂类制造具体产品，显然具体产品是作为返回类型。
实线：表示继承关系，子类指向父类。
简单工厂模式：一个工厂类分别生产每种具体产品，缺点是对修改不封闭，新增加产品要修改工厂。于是就有了工厂模式。
工厂模式：
工厂模式：抽象工厂有子类具体工厂ABC，分别用于生产对应的具体产品ABC。当新增加产品时，只需要再创建一个新的工厂子类。
2.特征点类
feature.h
同样，我们先分析Feature类的参数和函数组成，首先是参数，Feature类最主要的信息就是它在图像中的2d位置，是否为异常点等等，具体有以下参数：
- 持有该feature的frame
- 与该特征点关联的地图点
- 自身2d位置
- 是否异常
- 是否被左目相机提取
之后是构造函数，具体代码如下：
// feature类 含有自身的2d位置，是否异常，是否被左目相机提取
#pragma once
#ifndef MYSLAM_FEATURE_H
#define MYSLAM_FEATURE_H
#include "memory"
#include"opencv2/features2d.hpp"
#include"MYSLAM/common_include.h"
namespace MYSLAM{
struct Frame;
struct MapPoint;
// 2d特征点，三角化后会关联一个地图点
struct Feature
{
// 1.定义所需的参数
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
// 定义一个无符号类型的智能指针
typedef std::shared_ptr<Feature>ptr;
std::weak_ptr<Frame>frame_;// 持有该feature的frame
std::weak_ptr<MapPoint>map_point_;// 关联地图点
cv::KeyPoint pose_;// 自身2d位置
bool is_outlier_ =false;// 是否异常
bool is_on_left_image_=true;// 是否被左目相机提取
// 2.定义构造函数
public:
Feature(){};
// 构造函数
Feature(std::weak_ptr<Frame>frame , const cv::KeyPoint &kp): frame_(frame),pose_(kp){};
};
}
#endif // MYSLAM_FEATURE_Hfeature.cpp就比较简单
#include"MYSLAM/feature.h"
namespace MYSLAM{
}3.地图点类
MapPoint.h
有以下参数：
- ID
- 自身3D位置
- 是否是外点
- 数据锁
- 被哪些feature观察
- 被观测到的次数
构造函数：包括有参、无参构造
其他函数：
- 设置地图点的位置，并保证线程安全
- 取出地图点的位置，并保证线程安全
- 增加新的观测到这个路标点，并且特征点数量+1
- 可能是异常点，也可能将要删除某个关键帧，所以要移除某个特征点，并且特征点数量-1
- 工厂构建模式，分配id
// mappoint类包含 3d位置，被哪些feature观察
#pragma once
#ifndef MYSLAM_MAPPOINT_H
#define MYSLAM_MAPPOINT_H
#include"MYSLAM/common_include.h"
namespace MYSLAM{
struct Frame;
struct Feature;
// 地图点类，在三角化之后形成地图点
struct MapPoint
{
// 1.定义参数
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef std::shared_ptr<MapPoint>Ptr;// 定义了一个shared_ptr类型的指针
unsigned long id_ = 0;// ID
Vec3 pos_= Vec3::Zero();// 3D位置
bool is_outlier_=false;// 是否是外点
std::mutex data_mutex_;// 数据锁
std::list<std::weak_ptr<Feature>> observations_;//被哪些feature观察
int observed_times_=0;//被观测到的次数
// 2.构造函数和其他函数
public:
// （1）无参构造
MapPoint(){};
// （2）有参构造，输入id，3d位置
MapPoint(long id, Vec3 position);
// 取出地图点的位置，并保证线程安全
Vec3 Pos(){
std::unique_lock<std::mutex>lck(data_mutex_);
return pos_;
};
// 设置地图点的位置，并保证线程安全
void SetPos (const Vec3 &pos){
std::unique_lock<std::mutex>lck(data_mutex_);
pos_=pos;
}
// 增加新的观测到这个路标点，并且特征点数量+1
void AddObservation(std::shared_ptr<Feature>feature){
std::unique_lock<std::mutex>lck(data_mutex_);
observations_.push_back(feature);
observed_times_++;
}
// 可能是异常点，也可能将要删除某个关键帧，所以要移除某个特征点，并且特征点数量-1
void RemoveObservation(std::shared_ptr<Feature>feat);
// 工厂构建模式，分配id
static MapPoint::Ptr CreateNewMappoint();
};
} // namespace MYSLAM
#endif // MYSLAM_MAPPOINT_HMapPoint.cpp
包括工厂模式构造mappoint函数和RemoveObservation()的具体实现
#include"MYSLAM/mappoint.h"
#include"MYSLAM/feature.h"
namespace MYSLAM{
// 构造函数
MapPoint::MapPoint( long id, Vec3 position) : id_(id),pos_(position) {};
// 工厂模式
MapPoint::Ptr MapPoint::CreateNewMappoint() {
static long factory_id=0;
MapPoint::Ptr new_mappoint(new MapPoint);
new_mappoint->id_=factory_id++;
return new_mappoint;
}
// 可能是异常点，也可能将要删除某个关键帧，所以要移除某个特征点，并且特征点数量-1
void MapPoint::RemoveObservation(std::shared_ptr<Feature>feat){
std::unique_lock<std::mutex> lck(data_mutex_);//上锁
// 遍历observations_,找到被对应异常点观察到的那个feature
for(auto iter=observations_.begin();iter!=observations_.end() ; iter++){
if (iter->lock()==feat)
{
observations_.erase(iter);//从observations_,中删除
feat->map_point_.reset();//把对应的地图点删除
observed_times_--;//观察次数-1
break;//找到之后，删除完就可以跳出循环了
}
}
}
}// namespace MYSLAM至此三个基本的类抽象完毕，但是还缺一个地图类（Map）来管理frame和mappoint
4.地图类
Map.h
//map类与 frame、mappoint进行交互 ，维护一个被激活的关键帧和地图点
// 和地图的交互：前端调用InsertKeyframe和InsertMapPoint插入新帧和地图点，后端维护地图的结构，判定outlier/剔除等等
#pragma once
#ifndef MAP_H
#define MAP_H
#include "MYSLAM/common_include.h"
#include "MYSLAM/frame.h"
#include "MYSLAM/mappoint.h"
namespace MYSLAM{
// 开始定义Frame类
class Map{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef std::shared_ptr<Map>Ptr;// 无符号指针
// 为了方便查找，用哈希表的方式(容器)记录路标点、关键帧和被激活的关键帧，
// 输入id可以在O(1)时间内找到
typedef std::unordered_map<unsigned long,MapPoint::Ptr>LandmarksType;
typedef std::unordered_map<unsigned long,Frame::Ptr>KeyframesType;
// 无参构造
Map(){}
// 增加一个关键帧
void InsertKeyFrame(Frame::Ptr frame);
// 增加一个地图顶点
void InsertMapPoint(MapPoint::Ptr map_point);
// 获取所有地图点
LandmarksType GetAllMapPoints()
{
std::unique_lock<std::mutex> lck(data_mutex_);
return landmarks_;
}
// 获取激活的地图点
LandmarksType GetActiveMapPoints()
{
std::unique_lock<std::mutex> lck(data_mutex_);
return active_landmarks_;
}
// 获取激活关键帧
KeyframesType GetActiveKeyFrames()
{
std::unique_lock<std::mutex> lck(data_mutex_);
return active_keyframes_;
}
// 清理map中观测数量为0的点
void CleanMap();
private:
//将旧的关键帧置于不活跃的状态
void RemoveOldKeyframe();
std::mutex data_mutex_;// 数据锁
LandmarksType landmarks_;// 所有地图点
LandmarksType active_landmarks_;// 被激活的地图点
KeyframesType keyframes_;// 所有关键帧
KeyframesType active_keyframes_;// 被激活的关键帧
Frame::Ptr current_frame_ = nullptr;
int num_active_keyframes_=7;// 激活的关键帧数量
};
}//namespace MYSLAM
#endif // MAP_HMap.cpp
#include "MYSLAM/map.h"
#include "MYSLAM/feature.h"
namespace MYSLAM{
// 增加一个关键帧
void Map::InsertKeyFrame(Frame::Ptr frame){
current_frame_=frame;
//先在keyframe哈希表里找一下id，看看有没有添加过
// 如果没找到，就添加到keyframe和activeframe的哈希表中
if(keyframes_.find(frame->keyframe_id_)==keyframes_.end()){
keyframes_.insert( make_pair(frame->keyframe_id_,frame));
active_keyframes_.insert(make_pair(frame->keyframe_id_,frame));
}
// 如果该帧在之前已经添加进keyframe了，更新一下关键帧哈希表中的id
else{
keyframes_[frame->keyframe_id_]=frame;
active_keyframes_[frame->keyframe_id_] = frame;
}
// 如果活跃keyframe数量大于窗口限定数量7，则需要清除窗口内最old的那帧keyframe
if (active_keyframes_.size()>num_active_keyframes_)
{
RemoveOldKeyframe();//清除窗口内最old的那帧keyframe
}
}
// 增加一个地图顶点
void Map::InsertMapPoint(MapPoint::Ptr map_point){
//先在Landmarks哈希表里找一下id，看看有没有添加过
// 如果没找到，就添加到Landmarks和activeLandmarks的哈希表中
if (landmarks_.find(map_point->id_)==landmarks_.end())
{
landmarks_.insert(make_pair(map_point->id_,map_point));
active_landmarks_.insert(make_pair(map_point->id_,map_point));
}
//如果该地图点已经添加过了，就更新一下id
else{
landmarks_[map_point->id_]=map_point;
active_landmarks_[map_point->id_]=map_point;
}
}
//清除窗口内最old的那帧keyframe
void Map::RemoveOldKeyframe(){
if (current_frame_ == nullptr) return;
// 寻找与当前帧最近与最远的两个关键帧
int max_dis=0 , min_dis=9999; //定义最近距离和最远距离
int max_kf_id=0 , min_kf_id=0;//定义最近帧的id和最远帧的id
auto Twc=current_frame_->Pose().inverse();//定义Twc （）
// 遍历activekeyframe哈希表，计算每帧与当前帧的距离
for (auto &kf : active_keyframes_)
{
if (kf.second == current_frame_)
continue; // 如果遍历到当前帧自动跳过
// 计算每帧与当前帧的距离
auto dis = (kf.second->Pose() * Twc).log().norm();
// 如果距离>最远距离，则更新
if (dis > max_dis)
{
max_dis = dis;
max_kf_id = kf.first;
}
// 如果距离<最近距离，则更新
if (dis < min_dis)
{
min_dis = dis;
min_kf_id = kf.first;
}
}
const double min_dis_th = 0.2; // 设定一个最近阈值
Frame::Ptr frame_to_remove=nullptr;
if (min_dis<min_dis_th)
{
// 如果存在很近的帧，优先删掉最近的
frame_to_remove=keyframes_.at(min_kf_id);
}
// 否则 删掉最远的
else{
frame_to_remove=keyframes_.at(max_kf_id);
}
LOG(INFO) << "remove keyframe " << frame_to_remove->keyframe_id_;//打印删除的是哪一帧
// 确定好删除窗口中的哪一帧后，开始删除对应的关键帧和与之相关的地图点
active_keyframes_.erase(frame_to_remove->keyframe_id_);//删除窗口中的关键帧
// 遍历左目的特征点，将其删除
for (auto feat : frame_to_remove->features_left_)
{
auto mp = feat->map_point_.lock();
if (mp)
{
mp->RemoveObservation(feat);//移除左目特征点，并且特征点数量-1
}
}
// 遍历右目的特征点，将其删除
for (auto feat : frame_to_remove->features_right)
{
if (feat == nullptr) continue;
auto mp = feat->map_point_.lock();
if (mp)
{
mp->RemoveObservation(feat);//移除右边目特征点，并且特征点数量-1
}
}
CleanMap();// 清理map中观测数量为0的点
}
// 清理map中观测数量为0的点
void Map::CleanMap(){
int cnt_landmark_removed = 0;//设置被删除的点的次数
// 遍历窗口所有帧，如果该帧被观测的次数为0，则删除该帧
for(auto iter =active_landmarks_.begin(); iter != active_landmarks_.end();){
if (iter->second->observed_times_==0)
{
iter = active_landmarks_.erase(iter);
cnt_landmark_removed++;//记录次数+1
}
// 否则继续遍历
else{
++iter;
}
}
LOG(INFO) << "Removed " << cnt_landmark_removed << " active landmarks";//打印被删除的数量
}
} // namespace MYSLAM至此基础类已经抽象完毕，后续的类会根据系统推进而进行定义
![[dasctf]misc1](https://img-blog.csdnimg.cn/1db073a225094d30acd34470106edc7e.png)