Hand-shredded visual slam14 talks about ch13 code (2) abstraction of basic classes

Before officially writing the system, we analyzed the three basic classes in the previous article: frames, 2D feature points, and 3D map points. This time we started to code to implement these basic data structures:

1. Frame class

Frames in common SLAM systems need to contain the following information: id, pose, image, left and right eye feature points

frame.h:

In the process of abstraction, we divide it into the determination of parameters and functions. The first is the parameters:

• The id of the frame
• This frame serves as the id of the keyframe (keyframe)
• Whether it is a keyframe
• Timestamp
• TCW type pose (pose)
• pose data lock
• The binocular image that can be seen in this frame
• Binocular feature points that can be seen in this frame

After that comes the constructor, and some functional functions:

• No-argument constructor
• A parameterized constructor to initialize each parameter (input id, timestamp, pose, left and right eye images)

Since pose will be set or accessed by multiple threads at the front and back ends, the set and get functions of pose need to be defined, and a thread lock needs to be added when calling the function):

• set function: Set the pose of the frame and ensure thread safety
• get function: take out the pose of the frame and ensure thread safety

Finally, the Frame is built through the factory mode and the id is automatically assigned in the static function.

• factory build function

// 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

 Additional knowledge points:

About mutex locks:

• std::mutex: The programs in the book all use std::mutex, which is slightly different from pthread_mutex. std::mutex is a mutex lock implemented in C++ language. It has a very simple function and has cross-platform functions. If there are no special requirements for mutex locks, try to use std::mutex.
• unique_lock: unique-lock is a lock management template class. The unique-lock object manages the locking and unlocking operations of the mutex object with exclusive ownership, that is: within the declaration cycle of the unique-lock object, the lock object it manages It will remain locked, and after the unique-lock's declaration period ends, the object it manages will be unlocked. Therefore, the mutex hosted by unqie-lock does not have to consider its unlocking operation.

About smart pointers:

• shared_ptr: The problem solved by the smart pointer shared ptr: ensuring that the new dynamically allocated memory space can be released in each execution path of the program. By hosting the pointer returned by new to the shared-ptr object, you don't have to worry about where to write delete. In fact, you don't need to write delete yourself. The pointer will be automatically deleted when the shared-ptr object dies, and the shared-ptr object can be like Use it like an ordinary pointer. It can be simply understood as a smart pointer that can be automatically deleted.
• weak_ptr: weak-ptr is used to make up for the circular reference problem caused by the counting reference defect of shared-ptr. weak-ptr itself is also a template class, but it cannot be used to directly define smart pointers. It can only be used with share-ptr. You can use shared -ptr object is assigned to it without increasing the reference count, thereby avoiding the problem of reference rings (cyclic references).

About factory pattern:

Simple factory pattern:

Dotted line: indicates a dependency relationship. If A points to B, it means that A depends on B. That is, the attributes or methods of class B are used in class A, but the content of B is not changed. This refers to the factory class manufacturing specific products. Obviously the specific product is used as the return type.

Solid line: indicates inheritance relationship, subclass points to parent class.

Simple factory model: A factory class produces each specific product separately. The disadvantage is that it is not closed to modifications. Newly added products require modification of the factory. So there is the factory model.

Factory mode:

Factory pattern: Abstract factory has subclasses concrete factory ABC, which are used to produce corresponding specific products ABC. When adding a new product, you only need to create a new factory subclass.

2. Feature point class

feature.h

Similarly, we first analyze the parameters and function composition of the Feature class. The first is the parameters. The most important information of the Feature class is its 2D position in the image, whether it is an outlier, etc. The specific parameters are as follows:

• The frame holding the feature
• The map point associated with this feature point
• own 2d position
• Is it abnormal?
• Whether it is extracted by the left eye camera

After that comes the constructor, the specific code is as follows:

//  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 is relatively simple

#include"MYSLAM/feature.h"

namespace MYSLAM{

}

 3. Map point class

MapPoint.h

There are the following parameters:

•  ID
•  own 3D position
• Is it an external point?
• data lock
• Which features are observed?
• The number of times it was observed

Constructor: including parameterized and parameterless constructors

Other functions:

• Set the location of the map point and ensure thread safety
• Get the location of the map point and ensure thread safety
• Add new observations to this landmark point, and the number of feature points +1
• It may be an abnormal point, or a key frame may be deleted, so a feature point must be removed, and the number of feature points must be -1
• Factory build mode, assign 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

Including the specific implementation of the factory pattern construction mappoint function and 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

At this point, the three basic classes have been abstracted, but there is still a map class (Map) missing to manage frame and mappoint.

4. Map class

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

At this point, the basic classes have been abstracted, and subsequent classes will be defined as the system advances.