slslam----基于线特征的双目SLAM系统(关键知识点整理)

slslam----基于线特征的双目SLAM系统(关键知识点整理)

  • 1.注
  • 2. line 提取与跟踪
    • 2.1 简要
    • 2.2 主要代码
      • 2.2.1 line map 结构
      • 2.2.2 添加 line 进 line map
  • 3. 当前帧位姿估计
    • 3.1 简要
    • 3.2 主要代码 (BA)
      • 3.2.1 设置优化需要的参数索引
      • 3.2.2 build ceres
  • 4. 滑动窗口优化
    • 4.1 简要
    • 4.2 主要代码 (Local BA)
  • 5. 回环检测与位姿图优化

1.注

slslam 的论文 《Building a 3-D Line-Based Map Using Stereo SLAM》
项目代码部分开源,也是我很喜欢的一个双目线特征SLAM,但是代码不完整,并且代码的可读性较差,后续有时间我将会对这个系统进行重写,1)完善所有的代码工程 2)让整个框架可读性增高。 目前先对这个框架配合代码进行记录

2. line 提取与跟踪

2.1 简要

简要:提取使用lsd算法,跟踪采用lbd匹配,匹配包含前后帧左目线的匹配,以及右目线特征与左目线特征匹配。(为了后期方便计算,我们将提取到线特征端点坐标,转到归一化坐标系,代码中不是这样的,这里是为了方便介绍)。

经过匹配跟踪后,我们保留的线特征有两个特点:

  1. 每个线特征在左右目是成对出现的,并且被观测到的帧超过一帧,这样一个线路标,那么它在某一帧的左右目的观测下,有两条线,四个点,8个观测值
  2. 由于线特征是基于匹配进行跟踪的,这样导致观测到此线的帧都是连续的

2.2 主要代码

2.2.1 line map 结构

线路标结构中主要存储: 线特征的普吕克坐标六维度向量表示(其实不是完整的普吕克坐标),此坐标相对于它的初始帧位姿

map<int, landmark_t *> lm_map ; // line map
typedef struct
{
    Vector6d line;
    Vector2d tt;
    Vector3d pvn;
    bool twice_observed;
    bool ba_updated;
    bool currently_visible;
    int init_kfid;
    vector<obs_t> obs_vec;
} landmark_t;

2.2.2 添加 line 进 line map

// 根据 线在左右目的观测以及 基线,对线进行三角化,然后添加到地图
void SLAM::add_lms()
{

    int kfid = kfs.size() > 0 ? kfs.rbegin()->first : -1;
    // obit: 为 <线特征id,8维向量>

    for ( map<int, Vector8d> ::iterator obit = curr_obs.begin(); obit != curr_obs.end();
         ++obit)
    {
        // 查找观测对应的 线路标
        map<int, landmark_t *>::iterator lmit = lms.find(obit->first);
        
        // 若没有找到,说明这个线路标为 新的路标,则创建线路标
        if (lmit == lms.end())
        {
            landmark_t *lm = new landmark_t;
            lm->line = initialize_lm(obit->second, baseline); // baseline = 0.12  ; 双目 line 三角化
            lm->twice_observed = false;
            lm->ba_updated = false;
            lm->currently_visible = false;
            lm->tt = Vector2d(0, 0);
            lm->obs_vec.push_back(obs_t(kfid + 1, obit->second)); // 记录此观测:<关键帧的id ,对应的 对应的线特征 8维向量>
            lm->init_kfid = kfid + 1; // start_frame 帧
            Vector3d dv = lm->line.tail(3);  // d向量,直线的方向向量
            lm->pvn = gc_vec3_normalize(dv); // d向量,直线的方向向量 归一化
            lms.insert(make_pair(obit->first, lm));
        }
        else // 若非新线路标,则更新观测容器
        {
            lmit->second->obs_vec.push_back(obs_t(kfid + 1, obit->second));
        }
    }
}

线特征提取和跟踪完成之后,就可以把信息打包好,发给后续的位姿估计了。

3. 当前帧位姿估计

3.1 简要

提取到了当前帧的线特征,与上一帧的线特征进行比较,找到共同观测到的线特征,用这些线特征来计算当前帧的位姿,具体步骤为:

  1. 找到当前帧与上一帧共同观测到的线特征集合,假设有 n 对
  2. 在n对特征中随机选择一组,通过线的数学计算(线路标与相机平面之间的关系)计算出一个大概的相机位姿,然后对其他线特征进行重投影误差reprojection_error计算,得到内点,记录内点数量,这样迭代 n次, 返回内点最多对应的相机位姿。 (也就是RANSAC 算法
  3. 得到上一步骤的内点线特征以及初始的相机位姿,进行一次 BA ;位姿用六位向量表示,固定上一帧的位姿与内点线特征对应的线路标(4维向量正交表示),仅优化当前帧位姿
  4. 优化得到的位姿是相对于上一帧的位姿

3.2 主要代码 (BA)

主要介绍这儿的BA 估计当前帧相对于上一帧的位姿

3.2.1 设置优化需要的参数索引

注意

  1. 这儿当前帧位姿设置为前面 RANSAC 得到的初始位姿,然后再转为六维向量
  2. 上一帧的位姿设置为单位矩阵,计算出来的位姿就是相对于上一帧的位姿
vec_camera_param.push_back(gc_Rt_to_wt(T));        // 0  将待优化的位姿,转为 六维度的向量 装入相机参数
vec_camera_param.push_back(gc_Rt_to_wt(pose_t())); // 1   第二帧位姿 直接 设置为 单位矩阵,因为是固定第二帧位姿,仅优化 第一帧位姿  (第二帧为上一帧!!!)

    for (size_t i = 0; i < inliers.size(); i++)
    {
        // 这个观测涉及的一个相机位姿与线路标: 相机位姿不固定,线路标固定

        vec_camera_index.push_back(0);
        vec_line_index.push_back(fidx);
        vec_fixed_index.push_back(0);
        vec_fixed_index.push_back(1);
        vec_observations.push_back(obs1[inliers[i]]);

        // (这是上一帧)这个观测涉及的一个相机位姿与线路标: 相机位姿固定,线路标固定
        vec_camera_index.push_back(1);
        vec_line_index.push_back(fidx);
        vec_fixed_index.push_back(1);
        vec_fixed_index.push_back(1);
        vec_observations.push_back(obs0[inliers[i]]);

        vec_line_param.push_back(gc_av_to_orth(lines[inliers[i]])); // 线路标转为正交表示
        fidx++;
    }

3.2.2 build ceres

使用自动求导,LineReprojectionError 中残差为4维向量,对应4个观测点到各自的投影直线的距离值。观测值4个点,8个数,为归一化平面上的点

    void LBAProblem::build(Problem *problem)
    {
        const int _line_block_size = line_block_size();
        const int _camera_block_size = camera_block_size();
        double *lines = mutable_lines();
        double *cameras = mutable_cameras();

        const double *_observations = observations();

        for (int i = 0; i < num_observations(); ++i)
        {
            CostFunction *cost_function;

            cost_function = new AutoDiffCostFunction<LineReprojectionError, 4, 6, 4>(new LineReprojectionError(_observations[8 * i + 0],
                                                                                                               _observations[8 * i + 1],
                                                                                                               _observations[8 * i + 2],
                                                                                                               _observations[8 * i + 3],
                                                                                                               _observations[8 * i + 4],
                                                                                                               _observations[8 * i + 5],
                                                                                                               _observations[8 * i + 6],
                                                                                                               _observations[8 * i + 7]));

            // If enabled use Huber's loss function.
            // double focal_length = 320.0;
            double focal_length = 406.05;
            LossFunction *loss_function = robustify ? new HuberLoss(1.0 / focal_length) : NULL;
            //    LossFunction* loss_function = robustify ? new CauchyLoss(1.0) : NULL;

            double *camera = cameras + _camera_block_size * camera_index()[i];
            double *line = lines + _line_block_size * line_index()[i];

            problem->AddResidualBlock(cost_function, loss_function, camera, line);

            if (fixed_index()[2 * i])
                problem->SetParameterBlockConstant(camera);
            if (fixed_index()[2 * i + 1])
                problem->SetParameterBlockConstant(line);
        }
    }

4. 滑动窗口优化

4.1 简要

数据处理步骤:

  1. 找到满足窗口大小的帧,得到每一帧包含的线路标,以及它在这个窗口下被观测的次数,大于两次观测的线路表添加进 map lm_set 记录 ,第一个值对应 线路标在 line map 中的线id值,第二个值为被观测的次数。把所有窗口下的关键帧位姿添加到优化参数vec_camera_param中。
  2. 遍历这个 lm_set ,找到每个线路标,然后遍历这个线路标的观测帧,若这个观测帧不在窗口内,那么固定它的位姿。把线路标通过它的start_frame 位姿转到世界坐标系下,然后转为正交表示加入vec_line_param,观测添加进 vec_observations 中。
  3. 将所有参数 vec_observations ,vec_camera_param,vec_line_param,转为double 数组形式,进行ceres求解。同时还要标记,哪些线路标与帧是需要固定的。
  4. 优化完成之后,再把相机位姿更新,6维向量转为Rt ;线路标从世界坐标系下转到它的 start_frame 存储。

4.2 主要代码 (Local BA)

局部的滑动窗口 BA,构造如论文中图所示。 代码结构有点乱,其实若用g2o可能会更容易表示些。
slslam----基于线特征的双目SLAM系统(关键知识点整理)_第1张图片

    // 1.遍历 ba_kfs
    // 统计每一帧中线特征帧被观测次数
    // 每一帧加入 vec_camera_param ,kfid_map 记录
    //

    //  it : (kf全局id ,滑动窗口内或外的序号)
    for (mii::iterator it = ba_kfs.begin(); it != ba_kfs.end(); ++it)
    {
        // if ( it->second >= (int) ba_window_size )
        if (it->second >= (int)FLAGS_ba_window_size)
            continue;

        keyframe_t *kf = kfs[it->first];

        for (set<int>::iterator lmit = kf->member_lms.begin(); lmit != kf->member_lms.end(); ++lmit)
        {

            mii::iterator lit = lm_set.find(*lmit);

            if (lit == lm_set.end())
                lm_set.insert(make_pair(*lmit, 1));
            else
                lit->second++;
        }

        vec_kfs.push_back(kf);
        vec_camera_param.push_back(gc_Rt_to_wt(kf->T));
        kfid_map.insert(make_pair(it->first, kfidx++)); // ( kf的全局id,index )
    }

    int lmidx = 0;
    mii lmid_map;
    vector<landmark_t *> vec_lms;

    // 遍历ba_kfs中统计的 线 lm_set
    for (mii::iterator lit = lm_set.begin(); lit != lm_set.end(); ++lit)
    {
        if (lit->second < 2) // 过滤少于两次的观测
            continue;
        // lmit :(线路标的全局id,线路标指针对象)
        lm_map::iterator lmit = lms.find(lit->first); // 在总的线特征集合 lms 查看
        if (lmit == lms.end())                        // 首先要被找到
            continue;
        lmit->second->twice_observed = true;
        lmit->second->ba_updated = true;

        // 对找到的 线landmark 遍历其所有的观测
        for (size_t j = 0; j < lmit->second->obs_vec.size(); ++j)
        {

            // lmit :(线路标的全局id,线路标指针对象)

            obs_t &obt = lmit->second->obs_vec[j];

            if (ba_kfs.find(obt.id) == ba_kfs.end()) // 此观测所对应的 帧不是在 ba_kfs 中的,则过滤
                continue;

            // iit : (kf的全局id,index)
            mii::iterator iit = kfid_map.find(obt.id); // 查找 :帧与其对应的新序号
            if (iit == kfid_map.end())                 // 若此帧是未包含在 ba_kfs 中的,那么添加
            {
                vec_fixed_index.push_back(1); // 说明此帧,不属于滑动窗口内 !!! 需要固定
                vec_camera_index.push_back(kfidx);

                keyframe_t *kf = kfs[obt.id];

                vec_kfs.push_back(kf);
                vec_camera_param.push_back(gc_Rt_to_wt(kf->T)); // 加入到 相机优化参数数组中
                kfid_map.insert(make_pair(obt.id, kfidx++));
            }
            else // 此帧已经在 ba_kfs 中(滑动窗口中)
            {
                // if ( iit->second < (int) ba_window_size )
                if (iit->second < (int)FLAGS_ba_window_size) // 少于  FLAGS_ba_window_size,则需要优化
                    vec_fixed_index.push_back(0);
                else // 超过窗口大小,也设置为 固定
                    vec_fixed_index.push_back(1);
                vec_camera_index.push_back(iit->second); // 相机数量 index
            }
            // lmit :(线路标的全局id,线路标指针对象)

            //  lmid_map 为map : (线路标的全局id,线的新index)

            iit = lmid_map.find(lmit->first);
            if (iit == lmid_map.end())
            {
                vec_line_index.push_back(lmidx);
                //  lmid_map 为map : (线路标的全局id,线的新index)
                lmid_map.insert(make_pair(lmit->first, lmidx++));
            }
            else // 已经出现过,向向量中添加 线的index
            {
                vec_line_index.push_back(iit->second);
            }
            // 将在 滑动窗口内的 关键帧所对应的 线路标的观测添加进入 观测向量
            vec_observations.push_back(obt.obs);
        }
        // 将线(普吕克坐标)转到它第一次被观测的相机的坐标系下
        Vector6d line = gc_line_from_pose(lmit->second->line, kfs[lmit->second->init_kfid]->T);
        // 将 普吕克坐标转为 正交表示
        vec_line_param.push_back(gc_av_to_orth(line));
        vec_lms.push_back(lmit->second); // 把线路标加入 vec_lms 中
    }

5. 回环检测与位姿图优化

  1. 回环检测 基于 LBD 与 DBOW
  2. 位姿图优化通过添加帧与帧之间的边来构建pose graph

由于之前开源的代码也不完整,可读性一般,结构乱,等后面有时间了 我重写完这个工程,再说明吧。

你可能感兴趣的:(slam,SLAM,line,ceres,位姿估计,滑动窗口)