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

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可能会更容易表示些。

    // 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

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