VINS-FUSION的一些理解

关于滑窗中优化变量的理解:

Ps 、Vs 、Rs 代表的意义可能很多人还没有清楚,这个强调多少次都不为过!这个值是由IMU机械编排出来的,代表的是 b(k) 坐标系相对与世界坐标系的位置、速度、姿态,而这个k是对应的相机帧,因为相机和IMU是固连的 。而相机和IMU通过Rb_c进行转换的,也就是我们自动外参标定的部分!

组织特征点的方式:
在这里插入图片描述
在这里插入图片描述

关键帧选取规则

1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧(因为视差可以根据平移和旋转共同得到,而纯旋转则导致不能三角化成功,所以这一步需要IMU预积分进行补偿)
2、当前帧跟踪到的特征点数量小于阈值视为关键帧;
求取的视差是对应两帧中特征点的距离。

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    ROS_DEBUG("input feature: %d", (int)image.size());
    ROS_DEBUG("num of feature: %d", getFeatureCount());
    //所有特征点视差总和
    double parallax_sum = 0;
    // 满足某些条件的特征点个数
    int parallax_num = 0;
    //被跟踪点的个数
    last_track_num = 0;
    for (auto &id_pts : image)
    {
        //特征点管理器,存储特征点格式:首先按照特征点ID,一个一个存储,每个ID会包含其在不同帧上的位置
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
 
        int feature_id = id_pts.first;
       // find_if 函数,找到一个interator使第三个仿函数参数为真
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {
            return it.feature_id == feature_id;
                          } );
 
        if (it == feature.end())
        {
            //如果没有找到此ID,就在管理器中增加此特征点
            feature.push_back(FeaturePerId(feature_id, frame_count));
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        else if (it->feature_id == feature_id)
        {   
            //如果找到了相同ID特征点,就在其FeaturePerFrame内增加此特征点在此帧的位置以及其他信息,然后增加last_track_num,说明此帧有多少个相同特征点被跟踪到
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++;
        }
    }
 
    if (frame_count < 2 || last_track_num < 20)
        return true;
 
    for (auto &it_per_id : feature)
    {
        //计算能被当前帧和其前两帧共同看到的特征点视差
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;
        }
    }
 
    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
}
在这里插入double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
    //check the second last frame is keyframe or not
    //parallax betwwen seconde last frame and third last frame
    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];//这里怎么能确保feature_per_frame[frame_count - 1 - it_per_id.start_frame]总是存在的呢
 
    double ans = 0;
    Vector3d p_j = frame_j.point;// 3D路标点(倒数第二帧j)
 
    double u_j = p_j(0);
    double v_j = p_j(1);
 
    Vector3d p_i = frame_i.point;// 3D路标点(倒数第三帧i)
    Vector3d p_i_comp;
 
    //int r_i = frame_count - 2;
    //int r_j = frame_count - 1;
    //p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
    p_i_comp = p_i;
    double dep_i = p_i(2);
    double u_i = p_i(0) / dep_i;
    double v_i = p_i(1) / dep_i;
    double du = u_i - u_j, dv = v_i - v_j;
 
    //没有惯导补偿,这一部分重复了
    double dep_i_comp = p_i_comp(2);
    double u_i_comp = p_i_comp(0) / dep_i_comp;
    double v_i_comp = p_i_comp(1) / dep_i_comp;
    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;
 
    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));
 
    return ans;
}代码片

我的疑问:怎么知道FeaturePerId中的FeaturePerFrame的对应索引都存在呢

  • 0
    点赞
  • 9
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值