在正式写系统之前，我们在上一篇分析了基本的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_H

frame.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 position_;// 自身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),position_(kp){};
};
}

#endif  // MYSLAM_FEATURE_H

feature.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_H

MapPoint.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_H

Map.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

至此基础类已经抽象完毕，后续的类会根据系统推进而进行定义

手撕视觉slam14讲 ch13 代码（2）基本类的抽象

1.帧类

frame.h:

frame.cpp:

附加知识点：

2.特征点类

feature.h

feature.cpp就比较简单

3.地图点类

MapPoint.h

MapPoint.cpp

4.地图类

Map.h

Map.cpp

猜你喜欢

手撕 视觉slam14讲 ch13 代码（2）基本类的抽象

1.帧类

frame.h:

frame.cpp:

附加知识点：

2.特征点类

feature.h

feature.cpp就比较简单

3.地图点类

MapPoint.h

MapPoint.cpp

4.地图类

Map.h

Map.cpp

猜你喜欢

手撕视觉slam14讲 ch13 代码（2）基本类的抽象