民宿网站建设方案,吉林省示范校建设专题网站,建设工程公司简介模板,站长工具平台0. 内容 1. VIO回顾 整个视觉前端pipeline回顾#xff1a;
两帧图像#xff0c;可提取特征点#xff0c;特征匹配#xff08;描述子暴力匹配或者光流#xff09;已知特征点匹配关系#xff0c;利用几何约束计算relative pose([R|t])#xff0c;translation只有方向
两帧图像可提取特征点特征匹配描述子暴力匹配或者光流已知特征点匹配关系利用几何约束计算relative pose([R|t])translation只有方向没有尺度使用三角化获得3维坐标即可完成vslam系统的初始化有了3D特征点后续可根据特征跟踪使用PnP求解Camera Pose无需再使用几何约束 IMU的加速度要和世界系的重力进行对齐 世界系重力假设(00-9.81)IMU第一帧估计出一个 c g c R c b g b ^{c}g^{c}R_{cb}g_b cgcRcbgb就可以求出第一帧camera在world下的pose带头大哥 视觉的尺度和IMU尺度要进行对齐 VIO系统速度传感器bias需要估计 IMU和相机的外参Tic
知道了z轴的法向量就能估计roll和pitch(tilt)但是yaw不可观。
2. VINS鲁棒初始化
2.1 pipeline overview IMU积分是米制单位但是camera不是camera存在一个缩放因子标定出外参 [ R b c , t b c ] [R_{bc},t_{bc}] [Rbc,tbc]之后可以求得尺度因子s先对坐标系R,t的数学表示做一下说明 旋转脚标相连时相消刚性连接在不同时刻的的观测相同平移左上角是观测系右下角从左到右平移乘旋转后平移向量不变但观测系改变。
式(4)第一行这样理解 c 0 p ‾ c 0 b k c 0 p ‾ c 0 c k − 1 s c 0 R c 0 b k b p b c ( 2.1 ) {^{c_0}\overline p_{c_0b_k}}{^{c_0}\overline p_{c_0c_k}}-\frac{1}{s}{^{c_0} R_{c_0b_k}} {^{b}p_{bc}} (2.1) c0pc0bkc0pc0ck−s1c0Rc0bkbpbc(2.1) 左上角都是观测系即在什么系下看这个量由于刚性连接在不同观测系不同时间下相同且平移乘旋转后旋转向量不变但观测系改变所以 b p b c b k p b k c k ( 2.2 ) {^{b}p_{bc}}{^{b_k}p_{b_kc_k}}(2.2) bpbcbkpbkck(2.2) 所以将(2.2)带入式(2.1)可得 c 0 p ‾ c 0 b k c 0 p ‾ c 0 c k − c 0 p ‾ b k c k c 0 p ‾ c 0 c k c 0 p ‾ c k b k ( 2.3 ) {^{c_0}\overline p_{c_0b_k}}{^{c_0}\overline p_{c_0c_k}}-{^{c_0}\overline p_{b_kc_k}}{^{c_0}\overline p_{c_0c_k}}{^{c_0}\overline p_{c_kb_k}}(2.3) c0pc0bkc0pc0ck−c0pbkckc0pc0ckc0pckbk(2.3) 即在 c 0 c_0 c0系下进行将 c 0 p ‾ c 0 c k c 0 p ‾ c k b k {^{c_0}\overline p_{c_0c_k}}{^{c_0}\overline p_{c_kb_k}} c0pc0ckc0pckbk向量相加不难理解。 标定出Camera和IMU外参之后利用式(2.1)就可以求出尺度因子。 Tbc外参的Rotation很重要而Translation由于可能离得比较近所以相对来说没有Rotation重要
2.2 外参估计
旋转约束两种路径求取的 q c k b k 1 q_{c_kb_{k1}} qckbk1应该相同。 这里的未知量只有外参 q b c q_{bc} qbc也可以简单地理解为一段时间内camera出一段pose(trajectory)imu积分出一段pose(trajectory)这两段pose理想情况下只有外参的差异不考虑 t b c t_{bc} tbc的情况下可以将这两段pose align起来残差是两段pose align的残差待估计量就是外参做LSP的结果就是外参。 VINS论文中将多个时刻的数据累计起来并使用了鲁棒核函数对每一项进行了加权权值计算方法有 t r ( R ) 1 2 c o s θ tr(R)12cos\theta tr(R)12cosθ可以看wiki式(9)中的那一大块矩阵就是式5移项而得因为求的是相同的量如 q c k b k 1 q_{c_kb_{k1}} qckbk1所以理想情况下连乘应该是Identity但实际上如果外参估的不准这个角度会较大threshole所以式8将大于threshold的部分的权值设的较小进行了抑制。
最终对 q b c q_{bc} qbc的求解还是对式(7)使用SVD分解取 V T V^T VT最后一列作为解。
(参考这篇博客参考这篇博客中使用Lanrange算子证明取 V T V^T VT最后一列是我们所求的解。)
论文中还对SVD分解的倒数第二小奇异值进行了判断如果小于阈值则认为该次估计无效重新采集数据进行估计。这和增多数据来提升数据信噪比是一样的目的都是为了提高数值稳定性
2.3 gyro bias估计 式(10)就是将 b k b_k bk到 b k 1 b_{k1} bk1时刻之间imu的rotation都转到 c 0 c_0 c0系下进行align
前两项四元数相乘代表求在 c 0 c_0 c0系下看imu在 b k b_k bk到 b k 1 b_{k1} bk1时刻之间的relative rotation第3项 q b k b k 1 q_{b_kb_{k1}} qbkbk1是IMU积分而来的两时刻间的relative rotation真值会受到gyro bias的影响对其进行一阶Taylor展开带入(10)(10)即代表一阶项所代表的 θ e r r \theta_{err} θerr可以求出gyro bias
这跟前面估计外参的是一样的都是使用的旋转约束。
2.4 初始化速度重力尺度因子
