实例介绍
【实例简介】
VINS 的功能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程,分别是:前端图像跟踪、后端非线性优化(其中初始化和 IMU 预积分在这个线程中)、闭环检测、闭环优化。
、总体框架 Measurement Preprocessing Initialization Camera(30hz) Feature Detection nd racker Visual-lnertial Initialized is- only SfM Alignment IMU (100h MU Pre-integration Local Visual-Inertial: Oldest Sliting Window Newest Nonli +、 Keyframe? Optimization Bundle Adjustment I I Loop detection with Relocalization States from Loop Closure Fealure retrieve l oop Deleted 二二1---11------22=== Global Pose Graph 4-DoF Pose Graph Optimization Keyframe Database Optimization 图1VINS框架 ⅵINS的玏能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测 及闭环优化。代码中主要开启了四个线稈,分别是:前端图像跟踪、后端非线性伉化(其中 初始化和IMU预积分在这个线程中)、闭环检测、闭环优化 各个功能模块的作用上要有: 1.I图像和MU预处理 ●图像:提取图像 Harris角点,利用金字塔光流跟踪相邻帧,通过 RANSAC去除异常点, 最后将跟踪到的特征点push到图像队列中,并通知后端进行处理 ●IU:将IMU数据进行积分,得到当前时刻的位置、速度和旋转(PVQ),同时计算在 后端优化中将用到的相邻帧的预积分增量,及预积分误差的 Jacobian矩阵和协方差项。 1.2初始化 首先,利用SFM进行纯视觉佔计滑窗內所有帧的位姿及3D点逆深度,最后与IMU预 积分进行对齐求解初始化参数 1.3后端滑窗优化 将视觉约束、IMU约束和闭环约束放在·个大的目标函数中进行非线性优化,求解滑窗 内所有帧的PVQ、bias等。 L M States in the sliding window IMU: k States from loop clos 1 Camera: 冷 MU measurements >visual measurements ★ Catur 图2滑窗优化示意图 14闭环检测和优化 利用D)BoW进行闭环检测,当检测成功后进行重定位,最后对整个相机轨迹进行闭环优 化。 U预积分 Vision IMU Vision 图3MU预积分示意图 21当前时刻pVQ的连续形式 将第k唢和第kl帧之间的所有IMU进行积分,可得第kHI帧的位置、速度和旋转 (PVQ),作为视觉估计的初始值,这里的旋转采用的四元数。 v△t+ k+1 ∈[k,k+1] rW(at-ba ) -owlet b k JtE[k, k+1] n(,-b dt ∈[k,k+1] 其中,a2和O为ⅠMU测量的加速度和角速度,是在Body自身坐标系, world坐标系是 IMU所在的惯导系,上式的旋转公式推导可参考附录10.1。 22当前时刻PVQ的中值法离散形式 公式(1)给出的是连续吋刻的相机当前PVR的达代公式,为了跟代码致,下面给出基 于中值法的公式,这与 Estimator:; processIMg(O函数中的Ps]、Rs]和Vs是一致的,IMU 积分出来的第j时刻的物理量可以作为第j帧图像的初始值。 tr t +a26t (2) k a,St 其中 q(a1-ba)-g"+q:+1(a+1-ba) (a;+o;+1) 2.3两帧之间PVQ增量的连续形式 通过观察公式(1)可知,IvU的预积分需要依赖与第k帧的ν和R,当我们在后端进行 非线性优化时,需要迭代更新第κ唢的ν和R,这将导致我们需要根据每次迭代后值重新进 行积分,这将非常耗吋。因此,我们考虑将优化变量从第k帧到第κ+1帧的IU预积分项 中分离开来,通过对公式(1)左右两侧各乘Rb,可化简为: R (+p2k-=2△)+a k+1 b 其中 D tElk, k+1 t∈[k,k+1 R k(at-bar)ldt )Wendt t∈[kk+1 这样我们就得到了连续时刻的MU预积分公式,可以发现,上式得到的MU预积分的 值只与不同时刻的a2和o相关。 这里我们需要重新讨论下公式(5)预积分公式,以ab,为例,我们发现它是与MU的 bias相关的,而bias也是我们需要优化的变量,这将导致的问题是,当每次迭代时,我们得 到一个新的bias,又得根据公式(巧5)重新对第k帧和第k+1帧之间的IMU预积分,非常耗时。 这里假设预积分的变化量与bias是线性关系,可以写成: ab,+/6n6ba+/16b +8 8ba +p (6) k+1 sb 24两帧之间PVQ增量的欧拉法离散形式 面给出离散时刻的IMU预积分公式,首先按照论文中采用的欧拉法,给出第i个MU 时刻与第i1个IMU时刻的变量关系为 b +1 k+的6t+元R(P)( 1 +R(P")(a2-b bn)δt 25两帧之间PⅤQ增量的中值法离散形式 卜面给出代码中采用的基」中值法的IMU预积分公式,这与 Estimator: processIMUO函 数中的 Integration Base: push backo上是一致的。注意这里跟公式(2)是不一样的,这里积分 出来的是前后两顿之间的IU增量信息,而公式(2)给出的当前帧时刻的物理量信息 +1 +B k St +=a, &t b b +1 Bi + au 1 其中 a,=slqilai-bai)+qi Di t aitl 2.6连续形式下PVQ增量的误差、协方差及 Jacobian IMU在每个吋刻积分出来的值是有误差的,下面我们对误差进行分析。首先我们直 接给出在t时刻误差项的导数为: sa 0 0016a 0000 00-82( 066 hk R;000 6|=00-(a-bh) 0-1192k|+|0 00|m LL 00 0016ba 00101n 00 0018b 000 F+ozk+ Gt 其中:F25×15,G215×2,62x1,n12×,上式推导可参考附录102。下面我们讨论 它的作用,将其可以简写为: 6之k=F62z+Gtnt 根据导数定义可知:62b=1m24-62 62+8=62+628t=(+F6t)6z+(Gt6t)nt (11) 这里我们对公式(1)的IMU误差运动方程再说明,将上式和EKF对比可知,上式恰好 给出了如EKF一般对非线性系统线性化的过程,这里的意义是表示下一个时刻的IMU测 量误差与上一个时刻的成线性关系,这样我们根据当前时刻的值,可以预测出下一个时刻 的均值和协方差,而公式(1)给出的是均值预测,协方差预测公式如下 Pb+6=(1+Ft)P(+Fl6t)7+(G,t)Q(G18t) ot (12) 上式给出了协方差的选代公式,初始值Pk=0。其中,Q为表示噪声项的对角协方差 矩阵 000 00 3 000 另外根据(11)式可获得诀差项的 Jacobian的迭代公式: (I+F26t) (14) 其中 Jacobian的初始值为bk=1 2.7离散形式的PVQ增量误差分析 我们首先直接给出PVQ增量误差在离散形式下的矩阵形式,为了与代码一致,我们修 改下变量顺序,这和代码中 midPointIntegration(函数是一致的。(但不知为何计算的V中 与前四个噪声项相关的差个负号?) 1 t fo 66 0f10 6t‖loe Bk +1=0f211f2 0 016b δb 0 [6b 1 02 001rn ot 0 00 k Rkot k+1 (15 0 0 6t0 n 0 0 00δt 其中,推导可参考附录10.3: st E(ak-ba)02-4B+1( k k+1 21 k+1 b.)6t|6t2 (Rr+ rk+18t2 St n=71=Rk+1(a+1-b)6t Wr+ w f1=I k+1 1+G b。)δt-Rn+1(a k+1 21=-2 配+1 Stl st 2 1 (RK+Ruts) 4 rula 1 Rrot st St R+1(a 1 R,+1 1 4 /+1 1 t 28离散形式的PVQ增量误差的 Jacobian和协方差 将公式(15)简写为: k+1 F15×1582 15×1 +V15×13Q 则 Jacobian的迭代公式为 k+15×15=F/k (16) 其中, Jacobian的初始值为/k=l。这里计算出来的k+1只是为了给后面提供对bias的 acoblar。 协方差的迭代公式为 P+15×15=FPFr+vQv (17) 其中,初始值P=0。Q为表示噪声项的对角协方差矩阵: 00000 0 00aa000 Q18×18 0a00 (18) 000 000 三、后端非线性优化 31状态向量 状态向量共包括滑动窗口内的n+l1个所有相机的状态(包括位置、朝向、速度、加速度 计bias和陀螺仪bias)、 Camera到IMU的外参、m+1个3D点的逆深度 X=[ xr=pw,v b b pc,q 3.2目标函数 吗+(喻,2)+2 (19) 其中三个残差项即误差项分别为边缘化的先验信息、IMU测量残差、视觉的重投影残 差。三种残差都是用马氏距离表示。 根据《十四讲》中高斯牛顿法,若要计算目标函数的最小值,可以理解为,当优化变 量有一个增量后,目标函数值最小,以IU残差为例,可写成如下所示: nin lre2bk, X+8X r k x)+H SⅩ Dk+1 oX k+1 k+1 其中HB,为B关于 XIK Jacobian,将上式展开并令关于6X的导数为0,可得增量δx的 计算公式: H k 8X= k+1 TB 那么,公式(28)可写成 +∑ +∑ T k ∑ 1 rc 上式中,B为MU预积分噪声项的协方差,P为vual观测的噪声协方差。当MU的 噪声协方差P越大时,其信息矩阵Pk,将越小,意味着该MU观测越不可信,换句话 说,因MU噪声较大,越不可信IMU预积分数据,而更加相信 visual观测。注意,这里 的IMU和vsua协方差的绝对值没有意义,因为考虑得是两者的相对性 可将上式继续简化为: (Ap+AB +Acox=bp +bB +bc 其中,Ap,AB和Ac为 Hessian矩阵,上述方程称之为增量方程。 33MU约束 1)残差:两帧之间的PVQ和bias的变化量的差 △tx+ k+1 bk qb k+1 bR+1 xyz +g"△t)-Bk (20) sb b b b 其中各增量关于bias的 Jacobian可从公式(16)的大 Jacobian中的相应位置获得。上面与 代码中 Integration base: evaluateD对应, 2)优化变量 pb, 0W, Svb ,8ba:,bor Opb,, 80W ,Swb,, bakr, Sba 3)Jacobian: 计算 Jacobian时,残差对应求偏导对象分别为p6e,6vB,6h,ba], 6b,6b 【实例截图】
【核心代码】
VINS 的功能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程,分别是:前端图像跟踪、后端非线性优化(其中初始化和 IMU 预积分在这个线程中)、闭环检测、闭环优化。
、总体框架 Measurement Preprocessing Initialization Camera(30hz) Feature Detection nd racker Visual-lnertial Initialized is- only SfM Alignment IMU (100h MU Pre-integration Local Visual-Inertial: Oldest Sliting Window Newest Nonli +、 Keyframe? Optimization Bundle Adjustment I I Loop detection with Relocalization States from Loop Closure Fealure retrieve l oop Deleted 二二1---11------22=== Global Pose Graph 4-DoF Pose Graph Optimization Keyframe Database Optimization 图1VINS框架 ⅵINS的玏能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测 及闭环优化。代码中主要开启了四个线稈,分别是:前端图像跟踪、后端非线性伉化(其中 初始化和IMU预积分在这个线程中)、闭环检测、闭环优化 各个功能模块的作用上要有: 1.I图像和MU预处理 ●图像:提取图像 Harris角点,利用金字塔光流跟踪相邻帧,通过 RANSAC去除异常点, 最后将跟踪到的特征点push到图像队列中,并通知后端进行处理 ●IU:将IMU数据进行积分,得到当前时刻的位置、速度和旋转(PVQ),同时计算在 后端优化中将用到的相邻帧的预积分增量,及预积分误差的 Jacobian矩阵和协方差项。 1.2初始化 首先,利用SFM进行纯视觉佔计滑窗內所有帧的位姿及3D点逆深度,最后与IMU预 积分进行对齐求解初始化参数 1.3后端滑窗优化 将视觉约束、IMU约束和闭环约束放在·个大的目标函数中进行非线性优化,求解滑窗 内所有帧的PVQ、bias等。 L M States in the sliding window IMU: k States from loop clos 1 Camera: 冷 MU measurements >visual measurements ★ Catur 图2滑窗优化示意图 14闭环检测和优化 利用D)BoW进行闭环检测,当检测成功后进行重定位,最后对整个相机轨迹进行闭环优 化。 U预积分 Vision IMU Vision 图3MU预积分示意图 21当前时刻pVQ的连续形式 将第k唢和第kl帧之间的所有IMU进行积分,可得第kHI帧的位置、速度和旋转 (PVQ),作为视觉估计的初始值,这里的旋转采用的四元数。 v△t+ k+1 ∈[k,k+1] rW(at-ba ) -owlet b k JtE[k, k+1] n(,-b dt ∈[k,k+1] 其中,a2和O为ⅠMU测量的加速度和角速度,是在Body自身坐标系, world坐标系是 IMU所在的惯导系,上式的旋转公式推导可参考附录10.1。 22当前时刻PVQ的中值法离散形式 公式(1)给出的是连续吋刻的相机当前PVR的达代公式,为了跟代码致,下面给出基 于中值法的公式,这与 Estimator:; processIMg(O函数中的Ps]、Rs]和Vs是一致的,IMU 积分出来的第j时刻的物理量可以作为第j帧图像的初始值。 tr t +a26t (2) k a,St 其中 q(a1-ba)-g"+q:+1(a+1-ba) (a;+o;+1) 2.3两帧之间PVQ增量的连续形式 通过观察公式(1)可知,IvU的预积分需要依赖与第k帧的ν和R,当我们在后端进行 非线性优化时,需要迭代更新第κ唢的ν和R,这将导致我们需要根据每次迭代后值重新进 行积分,这将非常耗吋。因此,我们考虑将优化变量从第k帧到第κ+1帧的IU预积分项 中分离开来,通过对公式(1)左右两侧各乘Rb,可化简为: R (+p2k-=2△)+a k+1 b 其中 D tElk, k+1 t∈[k,k+1 R k(at-bar)ldt )Wendt t∈[kk+1 这样我们就得到了连续时刻的MU预积分公式,可以发现,上式得到的MU预积分的 值只与不同时刻的a2和o相关。 这里我们需要重新讨论下公式(5)预积分公式,以ab,为例,我们发现它是与MU的 bias相关的,而bias也是我们需要优化的变量,这将导致的问题是,当每次迭代时,我们得 到一个新的bias,又得根据公式(巧5)重新对第k帧和第k+1帧之间的IMU预积分,非常耗时。 这里假设预积分的变化量与bias是线性关系,可以写成: ab,+/6n6ba+/16b +8 8ba +p (6) k+1 sb 24两帧之间PVQ增量的欧拉法离散形式 面给出离散时刻的IMU预积分公式,首先按照论文中采用的欧拉法,给出第i个MU 时刻与第i1个IMU时刻的变量关系为 b +1 k+的6t+元R(P)( 1 +R(P")(a2-b bn)δt 25两帧之间PⅤQ增量的中值法离散形式 卜面给出代码中采用的基」中值法的IMU预积分公式,这与 Estimator: processIMUO函 数中的 Integration Base: push backo上是一致的。注意这里跟公式(2)是不一样的,这里积分 出来的是前后两顿之间的IU增量信息,而公式(2)给出的当前帧时刻的物理量信息 +1 +B k St +=a, &t b b +1 Bi + au 1 其中 a,=slqilai-bai)+qi Di t aitl 2.6连续形式下PVQ增量的误差、协方差及 Jacobian IMU在每个吋刻积分出来的值是有误差的,下面我们对误差进行分析。首先我们直 接给出在t时刻误差项的导数为: sa 0 0016a 0000 00-82( 066 hk R;000 6|=00-(a-bh) 0-1192k|+|0 00|m LL 00 0016ba 00101n 00 0018b 000 F+ozk+ Gt 其中:F25×15,G215×2,62x1,n12×,上式推导可参考附录102。下面我们讨论 它的作用,将其可以简写为: 6之k=F62z+Gtnt 根据导数定义可知:62b=1m24-62 62+8=62+628t=(+F6t)6z+(Gt6t)nt (11) 这里我们对公式(1)的IMU误差运动方程再说明,将上式和EKF对比可知,上式恰好 给出了如EKF一般对非线性系统线性化的过程,这里的意义是表示下一个时刻的IMU测 量误差与上一个时刻的成线性关系,这样我们根据当前时刻的值,可以预测出下一个时刻 的均值和协方差,而公式(1)给出的是均值预测,协方差预测公式如下 Pb+6=(1+Ft)P(+Fl6t)7+(G,t)Q(G18t) ot (12) 上式给出了协方差的选代公式,初始值Pk=0。其中,Q为表示噪声项的对角协方差 矩阵 000 00 3 000 另外根据(11)式可获得诀差项的 Jacobian的迭代公式: (I+F26t) (14) 其中 Jacobian的初始值为bk=1 2.7离散形式的PVQ增量误差分析 我们首先直接给出PVQ增量误差在离散形式下的矩阵形式,为了与代码一致,我们修 改下变量顺序,这和代码中 midPointIntegration(函数是一致的。(但不知为何计算的V中 与前四个噪声项相关的差个负号?) 1 t fo 66 0f10 6t‖loe Bk +1=0f211f2 0 016b δb 0 [6b 1 02 001rn ot 0 00 k Rkot k+1 (15 0 0 6t0 n 0 0 00δt 其中,推导可参考附录10.3: st E(ak-ba)02-4B+1( k k+1 21 k+1 b.)6t|6t2 (Rr+ rk+18t2 St n=71=Rk+1(a+1-b)6t Wr+ w f1=I k+1 1+G b。)δt-Rn+1(a k+1 21=-2 配+1 Stl st 2 1 (RK+Ruts) 4 rula 1 Rrot st St R+1(a 1 R,+1 1 4 /+1 1 t 28离散形式的PVQ增量误差的 Jacobian和协方差 将公式(15)简写为: k+1 F15×1582 15×1 +V15×13Q 则 Jacobian的迭代公式为 k+15×15=F/k (16) 其中, Jacobian的初始值为/k=l。这里计算出来的k+1只是为了给后面提供对bias的 acoblar。 协方差的迭代公式为 P+15×15=FPFr+vQv (17) 其中,初始值P=0。Q为表示噪声项的对角协方差矩阵: 00000 0 00aa000 Q18×18 0a00 (18) 000 000 三、后端非线性优化 31状态向量 状态向量共包括滑动窗口内的n+l1个所有相机的状态(包括位置、朝向、速度、加速度 计bias和陀螺仪bias)、 Camera到IMU的外参、m+1个3D点的逆深度 X=[ xr=pw,v b b pc,q 3.2目标函数 吗+(喻,2)+2 (19) 其中三个残差项即误差项分别为边缘化的先验信息、IMU测量残差、视觉的重投影残 差。三种残差都是用马氏距离表示。 根据《十四讲》中高斯牛顿法,若要计算目标函数的最小值,可以理解为,当优化变 量有一个增量后,目标函数值最小,以IU残差为例,可写成如下所示: nin lre2bk, X+8X r k x)+H SⅩ Dk+1 oX k+1 k+1 其中HB,为B关于 XIK Jacobian,将上式展开并令关于6X的导数为0,可得增量δx的 计算公式: H k 8X= k+1 TB 那么,公式(28)可写成 +∑ +∑ T k ∑ 1 rc 上式中,B为MU预积分噪声项的协方差,P为vual观测的噪声协方差。当MU的 噪声协方差P越大时,其信息矩阵Pk,将越小,意味着该MU观测越不可信,换句话 说,因MU噪声较大,越不可信IMU预积分数据,而更加相信 visual观测。注意,这里 的IMU和vsua协方差的绝对值没有意义,因为考虑得是两者的相对性 可将上式继续简化为: (Ap+AB +Acox=bp +bB +bc 其中,Ap,AB和Ac为 Hessian矩阵,上述方程称之为增量方程。 33MU约束 1)残差:两帧之间的PVQ和bias的变化量的差 △tx+ k+1 bk qb k+1 bR+1 xyz +g"△t)-Bk (20) sb b b b 其中各增量关于bias的 Jacobian可从公式(16)的大 Jacobian中的相应位置获得。上面与 代码中 Integration base: evaluateD对应, 2)优化变量 pb, 0W, Svb ,8ba:,bor Opb,, 80W ,Swb,, bakr, Sba 3)Jacobian: 计算 Jacobian时,残差对应求偏导对象分别为p6e,6vB,6h,ba], 6b,6b 【实例截图】
【核心代码】
标签：
好例子网口号：伸出你的我的手 — 分享！
小贴士
感谢您为本站写下的评论，您的评论对其它用户来说具有重要的参考价值，所以请认真填写。
- 类似“顶”、“沙发”之类没有营养的文字，对勤劳贡献的楼主来说是令人沮丧的反馈信息。
- 相信您也不想看到一排文字/表情墙，所以请不要反馈意义不大的重复字符，也请尽量不要纯表情的回复。
- 提问之前请再仔细看一遍楼主的说明，或许是您遗漏了。
- 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦，又没人会搭理，于人于己都无利。
关于好例子网
本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享，对搜索内容的合法性不具有预见性、识别性、控制性，仅供学习研究，请务必在下载后24小时内给予删除，不得用于其他任何用途，否则后果自负。基于互联网的特殊性，平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查；无论平台是否已进行审查，用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场，基于网友分享，根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定，若资源存在侵权或相关问题请联系本站客服人员，点此联系我们。关于更多版权及免责申明参见 版权及免责申明
网友评论
我要评论