一文详解VINS-Fusion初始化标定

作者丨卢涛@知乎

来源丨https://zhuanlan.zhihu.com/p/358232587

编辑丨3D视觉工坊

本文基于VINS-Fusion解释VIO系统的初始化,包括在线标定IMU-Camera的外参旋转,IMU角速度偏置,重力方向,单目尺度。单目初始化相比于双目,多一个构建SFM问题优化位姿、3D点的过程。如有错误,请您指正。

一、IMU与Camera外参旋转标定

只估计旋转  ,没有估计平移  ,平移通常可以指定,并且平移量很小,影响不如旋转大。原理就是常说的手眼标定  ,只不过这里只取旋转,丢掉了平移

推导很简单,用下面几个式子变换一下就能得到上面这个式子

对上面的式子做下变化,转换成  的形式

其中  是四元数的左乘、右乘矩阵,四元数向量相乘可以转换为一个矩阵与一个向量乘。得到上面这个形式就好办了,  用两帧图像间的IMU积分计算得到, 为两帧图像之间的位姿旋转,可以通过2d-2d计算H、E,恢复R。用多组数据通过SVD或者迭代最小二乘,就能把  算出来了,解  是  的SVD分解最小奇异值对应的右奇异向量。

在VINS中对每个  项还乘上了一个权重

 是camera旋转与IMU旋转对应的角度差,理论上角度差应该为0。VINS中角度差阈值  设为5°,如果旋转角度误差大于5°,该项会乘上一个系数  ,以降低该项的权重。相关函数如下,细节可以查看代码注释

https://github.com/smilefacehh/VINS-Fusion-DetailedNote

// initial_ex_rotation.cpp

/**
 * 在线标定外参旋转
 * 利用两帧之间的Camera旋转和IMU积分旋转,构建最小二乘问题,SVD求解外参旋转
 * 1、Camera系,两帧匹配点计算本质矩阵E,分解得到四个解,根据三角化成功点比例确定最终正确解R、t,得到两帧之间的旋转R
 * 2、IMU系,积分计算两帧之间的旋转
 * 3、根据旋转构建最小二乘问题,SVD求解外参旋转
 * @param corres            前一帧与当前帧匹配点
 * @param delta_q_imu       前一帧与当前帧IMU预积分得到的旋转
 * @param calib_ric_result  在线标定IMU与Camera之间的外参(旋转)
*/
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);

二、IMU角速度偏置标定

这一步只标定角速度偏置,没有标定加速度偏置,加速度偏置相对于重力加速度量级太小,影响不如角速度偏置大。疑问点,角速度偏置是在imu-camera外参标定之后才进行标定的,而外参标定过程中通过角速度积分计算IMU姿态旋转,是需要用到角速度偏置的,说明用的偏置是不精确的。个人解释是,角速度偏置本身量级很小,之所以需要精确的值,是因为长时间之后小误差被放大,而外参标定只在初始化的时候进行,小误差可以容忍。

在第一步得到IMU-Camera外参旋转之后,将camera的旋转通过外参变换到IMU系下,理论上这个旋转应该与IMU系下对应的旋转一致,差为0,但是由于误差(角速度偏置目前还是个估计值)的存在,不会为0,那么构建最小二乘问题,最小化旋转差量,优化角速度偏置

最小二乘问题,写出它的正规方程(normal equation)

 是旋转量残差,  是残差对优化变量的雅克比,此处我们只需要角速度偏置  ,所以从雅克比中取出旋转残差对与角速度偏置部分的雅克比即可。

VINS中用LDLT直接解算  ,然后更新  ,完成角速度偏置的标定

回过头来看,旋转只跟角速度偏置有关,跟加速度偏置没有关系,所以用旋转约束只能估计角速度偏置。

// IMU角速度偏置标定
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    // 从滑窗第一帧遍历到倒数第二帧
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;
    }
    delta_bg = A.ldlt().solve(b);

    // 更新偏置
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

    // 更新偏置之后,重新计算积分
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

三、标定重力向量、单目尺度

这一步要标定的是重力在第零帧下的坐标表示,单目的尺度,顺带还能标定IMU下的速度。旋转约束在前面标定外参、角速度偏置已经用过了。重力加速度用在速度、平移量中,尺度也是跟平移有关,所以这一步用速度、平移约束来标定。下面是IMU预积分中的平移项约束、速度项约束

将世界坐标系  换成零时刻相机坐标系 

代入

可以写成  的形式,待优化变量  包括各帧速度、  、  。目标是最小化平移、速度误差。在VINS里面也是直接通过LDLT解算  。

上面得到的  没有加入模长  限制,所以还要把这个限制加进来,在这个估计的基础上得到一个更精确的解,相应的尺度  也会进行调整。如何加入这个约束呢

 作为优化参数,那么优化后的重力加速度  就在原来 的附近空间里,相差不会很大。

还没完,VINS对优化后的  继续做了一点处理,还原了yaw角对应的旋转量。因为只靠平移、速度约束,yaw角是没有特定解的。只能将z轴方向对齐。如果旋转参与标定,yaw角是能求出来的。对应代码是LinearAlignment函数。

VINS初始化流程

上面标定的数据来自滑窗中所有帧,需要知道每帧的位姿,用图像帧的位姿变换与IMU的预积分量对齐,构建最小二乘问题求解外参、角速度偏置、重力加速度、尺度、速度。

对滑窗中的帧,构建SFM,优化位姿。具体流程如下,细节可参考代码注释。

/**
 * 系统初始化
 * 1、计算滑窗内IMU加速度的标准差,用于判断移动快慢
 * 2、在滑窗中找到与当前帧具有足够大的视差,同时匹配较为准确的一帧,计算相对位姿变换
 *   1) 提取滑窗中每帧与当前帧之间的匹配点(要求点在两帧之间一直被跟踪到,属于稳定共视点),超过20个则计算视差
 *   2) 两帧匹配点计算本质矩阵E,恢复R、t
 *   3) 视差超过30像素,匹配内点数超过12个,则认为符合要求,返回当前帧
 * 3、以上面找到的这一帧为参考系,Pnp计算滑窗每帧位姿,然后三角化所有特征点,构建BA(最小化点三角化前后误差)优化每帧位姿
 *   1) 3d-2d Pnp求解每帧位姿
 *   2) 对每帧与l帧、当前帧三角化
 *   3) 构建BA,最小化点三角化前后误差,优化每帧位姿
 *   4) 保存三角化点
 * 4、对滑窗中所有帧执行Pnp优化位姿
 * 5、Camera与IMU初始化,零偏、尺度、重力方向
*/
bool Estimator::initialStructure();

本文仅做学术分享,如有侵权,请联系删文。

下载1

在「3D视觉工坊」公众号后台回复:3D视觉即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复:3D视觉github资源汇总即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复:相机标定即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配即可下载独家立体匹配学习课件与视频网址。

重磅!3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球:针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近3000星球成员为创造更好的AI世界共同进步,知识星球入口:

学习3D视觉核心技术,扫描查看介绍,3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用,麻烦给个赞和在看~  

3D视觉工坊 CSDN认证博客专家 算法 3D视觉
个人公众号:3D视觉工坊。公众号特邀嘉宾及合伙人,先后就职于国内知名研究机构、自动驾驶公司、海康研究院,主要研究方向为深度学习、目标检测、语义分割、图像处理、自动驾驶感知算法等,CSDN博客专家。博主先后任职于国内知名研究院、知名大厂,致力于3D视觉算法、VLAM算法开发,涉及相机标定、手眼标定、结构光、点云后处理、三维重建等相关领域的研究,同时也是CSDN博客专家。3D视觉工坊坚持原创,近一年来输出了非常多的高质量文章,获得了粉丝的一致好评,我们将始终坚持走原创路线,打造一个铁杆粉丝的聚集区。
已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 书香水墨 设计师:CSDN官方博客 返回首页
实付 49.90元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值