손으로 찢은 시각적 슬램14는 ch13 코드(2) 기본 클래스의 추상화에 대해 이야기합니다.

시스템을 공식적으로 작성하기 전에 이전 기사에서 프레임, 2D 특징점, 3D 맵 포인트의 세 가지 기본 클래스를 분석했으며, 이번에는 이러한 기본 데이터 구조를 구현하기 위한 코딩을 시작했습니다.

1. 프레임 클래스

일반적인 SLAM 시스템의 프레임에는 ID, 포즈, 이미지, 왼쪽 및 오른쪽 눈 특징점 등의 정보가 포함되어야 합니다.

프레임.h:

추상화 과정에서 매개변수 결정과 기능 결정으로 나누어집니다. 첫 번째는 매개변수입니다.

• 프레임의 ID
• 이 프레임은 키프레임(keyframe)의 ID 역할을 합니다.
• 키프레임인지
• 타임스탬프
• TCW형 포즈(포즈)
• 데이터 잠금 포즈
• 이 프레임에서 볼 수 있는 쌍안경 이미지
• 이 프레임에서 볼 수 있는 쌍안경의 특징점

그 다음에는 생성자와 일부 기능 함수가 옵니다.

• 인수 없는 생성자
• 각 매개변수(입력 ID, 타임스탬프, 포즈, 왼쪽 및 오른쪽 눈 이미지)를 초기화하는 매개변수화된 생성자

포즈는 프런트엔드와 백엔드의 여러 스레드에 의해 설정되거나 액세스되므로 포즈의 설정 및 가져오기 함수를 정의해야 하며 함수 호출 시 스레드 잠금을 추가해야 합니다.

• 설정 기능: 프레임의 자세를 설정하고 스레드 안전성을 보장합니다.
• get 기능: 프레임의 포즈를 취하고 스레드 안전성을 보장합니다.

마지막으로 Factory 모드를 통해 Frame이 구축되고 static 함수에서 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

프레임.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: 고유 잠금은 잠금 관리 템플릿 클래스입니다. 고유 잠금 개체는 독점 소유권을 가진 뮤텍스 개체의 잠금 및 잠금 해제 작업을 관리합니다. 즉, 고유 잠금 개체의 선언 주기 내에서 이 개체가 관리하는 잠금 개체입니다. 잠금 상태를 유지하며 고유 잠금 선언 기간이 종료되면 관리하는 개체의 잠금이 해제됩니다. 따라서 unqie-lock이 호스팅하는 뮤텍스는 잠금 해제 작업을 고려할 필요가 없습니다.

스마트 포인터 정보:

• shared_ptr: 스마트 포인터 공유 ptr로 해결된 문제: 동적으로 할당된 새 메모리 공간이 프로그램의 각 실행 경로에서 해제될 수 있도록 보장합니다. new가 반환한 포인터를 shared-ptr 객체에 호스팅하면 어디에서 delete를 쓸지 걱정할 필요가 없습니다. 실제로 직접 delete를 쓸 필요도 없습니다. 포인터는 shared-ptr 개체에 자동으로 삭제됩니다. ptr 객체는 죽고 공유 ptr 객체는 다음과 같을 수 있습니다. 일반 포인터처럼 사용하세요. 간단히 말해서 자동으로 삭제가 가능한 스마트 포인터로 이해하시면 됩니다.
• weak_ptr: weak-ptr은 shared-ptr의 계산 참조 결함으로 인해 발생하는 순환 참조 문제를 보완하기 위해 사용됩니다.weak-ptr 자체도 템플릿 클래스이지만 스마트 포인터를 직접 정의하는 데 사용할 수는 없습니다. share-ptr과 함께 사용할 수 있습니다. 참조 카운트를 늘리지 않고 공유 -ptr 개체를 할당하여 참조 링(순환 참조) 문제를 피할 수 있습니다.

공장 패턴 정보:

간단한 팩토리 패턴:

점선: 의존 관계를 나타내며, A가 B를 가리킨다면 A가 B에 의존한다는 뜻이다. 즉, B 클래스의 속성이나 메소드를 A 클래스에서 사용하지만 B의 내용은 변경되지 않는다. 이는 특정 제품을 생산하는 공장 등급을 의미하며, 당연히 특정 제품이 반품 유형으로 사용됩니다.

실선: 상속 관계를 나타내며 하위 클래스는 상위 클래스를 가리킵니다.

단순 공장 모델: 공장 클래스는 각 특정 제품을 별도로 생산하지만 단점은 수정에 폐쇄적이지 않다는 점이며, 새로 추가되는 제품은 공장 수정이 필요합니다. 그래서 공장 모델이 있습니다.

공장 모드:

팩토리 패턴: 추상 팩토리에는 해당하는 특정 제품 ABC를 생산하는 데 사용되는 구체적인 팩토리 ABC의 서브클래스가 있습니다. 새 제품을 추가할 때 새 팩토리 하위 클래스만 생성하면 됩니다.

2. 특징점 클래스

기능.h

마찬가지로 먼저 Feature 클래스의 매개변수와 기능 구성을 분석합니다. 첫 번째는 매개변수입니다. Feature 클래스의 가장 중요한 정보는 이미지에서의 2D 위치, 이상값인지 여부 등입니다. 구체적인 매개변수는 다음과 같습니다. 다음과 같습니다:

• 특징을 담고 있는 프레임
• 이 특징점과 연관된 지도 점
• 자신의 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 위치
• 외부점인가요?
• 데이터 잠금
• 어떤 특징이 관찰되나요?
• 관찰된 횟수

생성자: 매개변수가 있는 생성자와 매개변수가 없는 생성자를 포함합니다.

기타 기능:

• 지도 지점의 위치를 ​​설정하고 스레드 안전성을 보장합니다.
• 지도 지점의 위치를 ​​파악하고 스레드 안전성을 보장합니다.
• 이 랜드마크 포인트에 새로운 관측치를 추가하면 특징 포인트 수가 +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

팩토리 패턴 구성 맵포인트 함수 및 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)가 여전히 누락되어 있습니다.

4. 지도 클래스

지도.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

지도.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

이 시점에서 기본 클래스는 추상화되었으며, 시스템이 발전함에 따라 후속 클래스를 정의하게 됩니다.