VINS-MONO代码解读---特征点管理feature_manager.cpp

最新推荐文章于 2023-03-02 11:21:31 发布

可即

最新推荐文章于 2023-03-02 11:21:31 发布

阅读量1.5k

点赞数 7

分类专栏： SLAM VINS-MONO 文章标签： slam 自动驾驶

本文链接：https://blog.csdn.net/xiaojinger_123/article/details/119386705

版权

本文对estimator.cpp中的feature_manager.cpp进行介绍，主要是对特征点管理。
特征点管理器主要就是FeatureManager类
在这里插入图片描述涉及到的类的成员函数：

一、FeatureManager、FeaturePerId、FeaturePerFrame

1、三者关系
f_manager是特征管理器类的对象

FeatureManager f_manager;//特征管理器类

这里f_mangager.feature主要指的是一个list容器

feature_manager.h主要三个类：

FeatureManager管理所有特征点，通过list容器存储特征点属性

FeaturePerId指的是某feature_id下的所有FeaturePerFrame。常用feature_id和观测第一帧start_frame、最后一帧endFrame()

FeaturePerFrame指的是每帧基本的数据：特征点[x,y,z,u,v,vx,vy]和td IMU与cam同步时间差
在这里插入图片描述 **2、FeaturePerId，某feature_id下的所有FeaturePerFrame **
feature_id 特征点ID、start_frame 出现该角点的第一帧的id–start_frame

class FeaturePerId
{
   
  public:
    const int feature_id;// 特征点ID索引
    int start_frame;// 首次被观测到时，该帧的索引
    vector<FeaturePerFrame> feature_per_frame; // 能够观测到某个特征点的所有相关帧
 
    int used_num;// 该特征出现的次数
    bool is_outlier;// 是否外点
    bool is_margin;// 是否Marg边缘化
    double estimated_depth; // 估计的逆深度
    int solve_flag; // 求解器 0 haven't solve yet; 1 solve succ; 2 solve fail;
 
    Vector3d gt_p; // ？？？
 
    FeaturePerId(int _feature_id, int _start_frame) 
        : feature_id(_feature_id), start_frame(_start_frame),
          used_num(0), estimated_depth(-1.0), solve_flag(0)
    {
   
    }
 
    int endFrame();// 返回最后一次观测到这个特征点的帧数ID
};

** 3、FeaturePerFrame**
_point 每帧的特征点[x,y,z,u,v,vx,vy]， td IMU和cam同步时间差

class FeaturePerFrame
{
   
  public:
    FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td)
    {
   
        point.x() = _point(0);
        point.y() = _point(1);
        point.z() = _point(2);
        uv.x() = _point(3);
        uv.y() = _point(4);
        velocity.x() = _point(5); 
        velocity.y() = _point(6); 
        cur_td = td;
    }
    double cur_td;
    Vector3d point;
    Vector2d uv;
    Vector2d velocity;
 
    double z; // 特征点的深度
    bool is_used;// 是否被用了
    double parallax;// 视差
    MatrixXd A; //变换矩阵
    VectorXd b;
    double dep_gradient; // ？？？
};

三者串联最好的例子是：从f_manager到it_per_id再到底层的it_per_frame，就可以得到基本数据point了

for (auto &it_per_id : f_manager.feature)
{
   
   ......
   for (auto &it_per_frame : it_per_id.feature_per_frame)
   {
   
     Vector3d pts_j = it_per_frame.point;// 3D特征点坐标
 }
}

class FeatureManager中 list<FeaturePerId> feature;// 重要！！ 通过FeatureManager可以得到滑动窗口内所有的角点信息

class FeaturePerId中 vector<FeaturePerFrame> feature_per_frame; // 能够观测到某个特征点的所有相关帧

二、主要函数

1、endFrame()
返回最后一个观测到这个特征点的图像帧ID

int FeaturePerId::endFrame()
{
   
    return start_frame + feature_per_frame.size() - 1;
}

其中，feature_per_frame的数量为vector容器中，FeaturePerFrame基本类的数量。代表能够观测到某个特征点的所有帧。

vector<FeaturePerFrame> feature_per_frame; // 能够观测到某个特征点的所有相关帧

2、getFeatureCount()

窗口中被跟踪特征点的数量。
标准：该特征点被两帧以上观测到了，且第一次观测到的帧数不是在最后面。

int FeatureManager::getFeatureCount()
{
   
    int cnt = 0;
    for (auto &it : feature)// 遍历feature
    {
   
 
        it.used_num = it.feature_per_frame.size(); // 所有特征点被观测到的帧数
        // 如果该特征点有两帧以上观测到了  且第一次观测到帧数不是在最后
        if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)
        {
   
            cnt++;// 这个特征点是有效的
        }
    }
    return cnt;
}