专栏名称: 点云PCL

公众号将会推送基于PCL库的点云处理，SLAM，三维视觉，高精地图相关的文章。公众号致力于理解三维世界相关内容的干货分享。不仅组织技术交流群，而且组建github组群，有兴趣的小伙伴们可以自由的分享。欢迎关注参与交流或分享。

【VINS论文笔记】系列之视觉惯性联合初始化

点云PCL · 公众号 · · 2021-02-16 08:00

正文

点云PCL免费知识星球，点云论文速读。

标题： VINS-Mono代码解读——视觉惯性联合初始化 initialStructure sfm

作者： Manii

来源：https://blog.csdn.net/qq_41839222/category_9286052.html

排版：点云PCL

本文仅做学术分享，已获得作者授权转载，未经允许请勿二次转载！欢迎各位加入免费知识星球，获取PDF文档，欢迎转发朋友圈，分享快乐。

希望有更多的小伙伴能够加入我们，一起开启论文阅读，相互分享的微信群。参与和分享的方式：[email protected]

前言

本文主要介绍VINS状态估计器模块（estimator）中的初始化过程（initial），对应论文第五章（V. ESTIMATOR INITIALIZATION），主要在代码中/vins_estimator节点的相关部分实现。

由于单目紧耦合的VIO是一个高度非线性系统，单目视觉没有尺度信息，IMU的测量又存在偏置误差，如果没有良好的初始值很难将这两种测量结果有机融合，因而初始化是VIO最脆弱的步骤。

VINS采用了视觉和IMU的松耦合初始化方案，首先用从运动恢复结构（SFM）得到纯视觉系统的初始化，即滑动窗口中所有帧的位姿和所有路标点的3d位置，然后将其与IMU预积分结果进行对齐，恢复尺度因子、重力、陀螺仪偏置和每一帧的速度。

考虑初始化的原理和代码实现结合的比较紧密，决定将论文内容放到具体函数出现时再进行解释。

另外我认为要看懂这部分代码，很重要的一点是你要知道每个变量的具体含义，尤其是矩阵R（或四元数Q）和向量T，它代表的是哪一帧相对于什么坐标系的位姿，或是什么坐标系转换到什么坐标系下的变换矩阵。代码中有大量求对称的变换矩阵以及openCV函数求解RT，需要明白其到底在做什么。

流程图

代码实现

代码主要包含在estimator.cpp以及initial文件夹中，有关在初始化前的一些流程可以参考我的上一篇博客VINS-Mono代码解读——状态估计器流程。
这里直接从estimator.cpp中的 if (solver_flag == INITIAL) 开始解释。

初始化入口

frame_count表示目前滑动窗口中图像帧的数量，一开始初始化为0，WINDOW_SIZE=10表示滑动窗口中存储第0帧~第10帧的信息，这里目的是为了确保有足够的frame参与初始化。

  if (frame_count == WINDOW_SIZE)        {            bool result = false;            //有外参且当前帧时间戳大于初始化时间戳0.1秒，就进行初始化操作            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)            {               //视觉惯性联合初始化               result = initialStructure();               //更新初始化时间戳               initial_timestamp = header.stamp.toSec();            }            if(result)//初始化成功            {                //先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿                solver_flag = NON_LINEAR;                solveOdometry();                slideWindow();                f_manager.removeFailures();                ROS_INFO("Initialization finish!");                last_R = Rs[WINDOW_SIZE];                last_P = Ps[WINDOW_SIZE];                last_R0 = Rs[0];                last_P0 = Ps[0];                    }            else                slideWindow();//初始化失败则直接滑动窗口        }        else            frame_count++;//图像帧数量+1

initialStructure() 视觉惯性联合初始化

1、通过计算线加速度的标准差，判断IMU是否有充分运动激励，以进行初始化
注意这里并没有算上all_image_frame的第一帧，所以求均值和标准差的时候要减一

map::iterator frame_it;        Vector3d sum_g;        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++){            double dt = frame_it->second.pre_integration->sum_dt;            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;            sum_g += tmp_g;        }        Vector3d aver_g;        aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);//均值        double var = 0;        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)        {            double dt = frame_it->second.pre_integration->sum_dt;            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;            var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);        }        var = sqrt(var / ((int)all_image_frame.size() - 1));//标准差        if(var < 0.25)            ROS_INFO("IMU excitation not enouth!");

2、将f_manager中的所有feature保存到vector sfm_f中，代码略
这里解释一下SFMFeature，其存放的是特征点的信息

3、保证具有足够的视差,由E矩阵恢复R、t
这里的第L帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，会作为参考帧到下面的全局sfm使用，得到的Rt为当前帧到第l帧的坐标系变换Rt

4、对窗口中每个图像帧求解sfm问题，得到所有图像帧相对于参考帧的旋转四元数Q、平移向量T和特征点坐标sfm_tracked_points。

5、对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行优化

6、进行视觉惯性联合初始化

    if (visualInitialAlign())        return true;    else    {        ROS_INFO("misalign visual structure with IMU");        return false;    }

bool relativePose(relative_R, relative_T, l)

该函数判断每帧到窗口最后一帧对应特征点的平均视差大于30，且内点数目大于12则可进行初始化，同时返回当前帧到第l帧的坐标系变换R和T

bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l){    for (int i = 0; i < WINDOW_SIZE; i++)    {        //寻找第i帧到窗口最后一帧的对应特征点        vector> corres;        corres = f_manager.getCorresponding(i, WINDOW_SIZE);        if (corres.size() > 20){            //计算平均视差            double sum_parallax = 0;            double average_parallax;            for (int j = 0; j < int(corres.size()); j++){                //第j个对应点在第i帧和最后一帧的(x,y)                Vector2d pts_0(corres[j].first(0), corres[j].first(1));                Vector2d pts_1(corres[j].second(0), corres[j].second(1));                double parallax = (pts_0 - pts_1).norm();                sum_parallax = sum_parallax + parallax;            }            average_parallax = 1.0 * sum_parallax / int(corres.size());            //判断是否满足初始化条件：视差>30和内点数满足要求(大于12)            //solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够            //同时返回窗口最后一帧（当前帧）到第l帧（参考帧）的relative_R，relative_T            if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T)){                l = i;                ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);                return true;            }        }    }    return false;}

在m_estimator.solveRelativeRT(corres, relative_R, relative_T)中主要用到了两个opencv函数，这里稍微解释一下：

bool sfm.construct()

纯视觉sfm，求解窗口中所有图像帧的位姿QT（相对于第l帧）和特征点坐标sfm_tracked_points

bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,  const Matrix3d relative_R, const Vector3d relative_T,  vector &sfm_f, map<int, Vector3d> &sfm_tracked_points)

frame_num=frame_count + 1=11，frame_num-1表示当前帧
1、这里把第l帧看作参考坐标系，根据当前帧到第l帧的relative_R，relative_T，得到当前帧在参考坐标系下的位姿，之后的pose[i]表示第l帧到第i帧的变换矩阵[R|T]

2、先三角化第l帧（参考帧）与第frame_num-1帧（当前帧）的路标点
3、pnp求解参考坐标系到第l+1开始的每一帧的变换矩阵R_initial, P_initial，保存在Pose中。
并与当前帧进行三角化。

4、对第l帧与从第l+1到frame_num-2的每一帧再进行三角化

5、PNP求解参考坐标系到从第l-1到第0帧的每一帧之间的变换矩阵，并进行三角化

6、三角化其他未恢复的特征点。至此得到了滑动窗口中所有图像帧的位姿以及特征点的3d坐标

7、使用cares进行全局BA优化，代码略

8、这里得到的是第l帧坐标系到各帧的变换矩阵，应将其转变为每一帧在第l帧坐标系上的位姿

bool Estimator::visualInitialAlign() 视觉惯性联合初始化

该函数主要实现了陀螺仪的偏置校准(加速度偏置没有处理)，计算速度V[0:n]、重力g、尺度s。同时更新了Bgs后，IMU测量量需要repropagate；得到尺度s和重力g的方向后，需更新所有图像帧在世界坐标系下的Ps、Rs、Vs。

1、计算陀螺仪偏置，尺度，重力加速度和速度

    bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);    if(!result)    {        ROS_DEBUG("solve g failed!");        return false;    }

2、获取所有图像帧的位姿Ps、Rs，并将其置为关键帧

3、重新计算所有特征点的深度

4、陀螺仪的偏置bgs改变，重新计算预积分

  for (int i = 0; i <= WINDOW_SIZE; i++)    {        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);    }

5、将Ps、Vs、depth尺度s缩放后转变为相对于第0帧图像坐标系下

6、通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff

    Matrix3d R0 = Utility::g2R(g);    double yaw = Utility::R2ypr(R0 * Rs[0]).x();    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;    g = R0 * g;    Matrix3d rot_diff = R0;

7、所有变量从参考坐标系c0旋转到世界坐标系w

  for (int i = 0; i <= frame_count; i++)    {        Ps[i] = rot_diff * Ps[i];        Rs[i] = rot_diff * Rs[i];        Vs[i] = rot_diff * Vs[i];    }

至此VINS初始化的代码流程已经结束了，但是最关键的：
VisualIMUAlignment(all_image_frame, Bgs, g, x) 如何求解陀螺仪偏置、速度、重力和尺度还没有讲，而这才是VIO初始化的关键。

资源

三维点云论文及相关应用分享

【点云论文速读】基于激光雷达的里程计及3D点云地图中的定位方法

3D目标检测：MV3D-Net

三维点云分割综述（上）

3D-MiniNet: 从点云中学习2D表示以实现快速有效的3D LIDAR语义分割（2020）

win下使用QT添加VTK插件实现点云可视化GUI

JSNet：3D点云的联合实例和语义分割

大场景三维点云的语义分割综述

PCL中outofcore模块---基于核外八叉树的大规模点云的显示

更多文章可查看：点云学习历史文章大汇总

SLAM及AR相关分享

【开源方案共享】ORB-SLAM3开源啦！

【论文速读】AVP-SLAM：自动泊车系统中的语义SLAM

【点云论文速读】StructSLAM:结构化线特征SLAM

SLAM和AR综述

常用的3D深度相机

AR设备单目视觉惯导SLAM算法综述与评价

【VINS论文笔记】系列之视觉惯性联合初始化

正文

请到「今天看啥」查看全文