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

在正式写系统之前,我们在上一篇分析了基本的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 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> features_left_;
                std::vector> 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 lck(pose_mutex_);//线程锁
                        return pose_;
        }

        // 设置帧的位姿,并保证线程安全
        void SetPose(const SE3 &pose){
            std::unique_lock  lck(pose_mutex_) ;//线程锁
            pose_=pose;
        }

        // 设置关键帧并分配并键帧id
        void SetKeyFrame();

        // 工厂构建模式,分配id 
        static  std::shared_ptrCreateFrame();
};
}

#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的对象赋值给它,而不增加引用计数,从而避免引用成环(循环引用)的问题。

关于工厂模式:

简单工厂模式:

手撕 视觉slam14讲 ch13 代码(2)基本类的抽象_第1张图片

虚线:表示依赖关系,A指向B,则代表A依赖于B,即A类中使用了B类的属性或方法,但是不改变B的内容。这里指的是工厂类制造具体产品,显然具体产品是作为返回类型。

实线:表示继承关系,子类指向父类。

简单工厂模式:一个工厂类分别生产每种具体产品,缺点是对修改不封闭,新增加产品要修改工厂。于是就有了工厂模式。

工厂模式:

手撕 视觉slam14讲 ch13 代码(2)基本类的抽象_第2张图片

工厂模式:抽象工厂有子类具体工厂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_ptrptr;
      
        std::weak_ptrframe_;// 持有该feature的frame
        std::weak_ptrmap_point_;// 关联地图点
        cv::KeyPoint pose_;// 自身2d位置
        bool is_outlier_ =false;// 是否异常
        bool is_on_left_image_=true;//  是否被左目相机提取


// 2.定义构造函数
    public:
        Feature(){};
        // 构造函数
        Feature(std::weak_ptrframe , const cv::KeyPoint &kp): frame_(frame),pose_(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_ptrPtr;// 定义了一个shared_ptr类型的指针
        unsigned long id_ = 0;// ID
        Vec3 pos_= Vec3::Zero();// 3D位置
        bool is_outlier_=false;// 是否是外点
        std::mutex data_mutex_;// 数据锁
        std::list> observations_;//被哪些feature观察
        int observed_times_=0;//被观测到的次数

// 2.构造函数和其他函数
    public:
    // (1)无参构造
        MapPoint(){};
    // (2)有参构造,输入id,3d位置
        MapPoint(long id,  Vec3 position);

    // 取出地图点的位置,并保证线程安全
        Vec3 Pos(){
            std::unique_locklck(data_mutex_);
            return  pos_;
        };

    //  设置地图点的位置,并保证线程安全
        void SetPos (const Vec3 &pos){
            std::unique_locklck(data_mutex_);
            pos_=pos;
        }

    // 增加新的观测到这个路标点,并且特征点数量+1
        void AddObservation(std::shared_ptrfeature){
            std::unique_locklck(data_mutex_);
            observations_.push_back(feature);
            observed_times_++;
        }   

    // 可能是异常点,也可能将要删除某个关键帧,所以要移除某个特征点,并且特征点数量-1
        void RemoveObservation(std::shared_ptrfeat);

     // 工厂构建模式,分配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_ptrfeat){
    std::unique_lock 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_ptrPtr;// 无符号指针
    // 为了方便查找,用哈希表的方式(容器)记录路标点、关键帧和被激活的关键帧,
    // 输入id可以在O(1)时间内找到
    typedef std::unordered_mapLandmarksType;
    typedef std::unordered_mapKeyframesType;
    
    // 无参构造
    Map(){}

    // 增加一个关键帧
    void InsertKeyFrame(Frame::Ptr frame);

    // 增加一个地图顶点
    void InsertMapPoint(MapPoint::Ptr map_point);

    // 获取所有地图点
    LandmarksType GetAllMapPoints()
    {
        std::unique_lock lck(data_mutex_);
        return landmarks_;
    }

    // 获取激活的地图点
    LandmarksType GetActiveMapPoints()
    {
        std::unique_lock lck(data_mutex_);
        return active_landmarks_;
    }

    // 获取激活关键帧
    KeyframesType GetActiveKeyFrames()
    {
        std::unique_lock 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_diskeyframe_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

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

你可能感兴趣的:(手撕VO篇,视觉slam十四讲,计算机视觉,SLAM,ubuntu)