VINS论文推倒及代码解析
VINS 的功能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程,分别是:前端图像跟踪、后端非线性优化(其中初始化和 IMU 预积分在这个线程中)、闭环检测、闭环优化。、总体框架Measurement PreprocessingInitializationCamera(30hz)Feature Detectionnd rackerVisual-lnertialInitializedis- onlySfMAlignmentIMU (100hMU Pre-integrationLocal Visual-Inertial: OldestSliting WindowNewestNonli+、 Keyframe?OptimizationBundle Adjustment II Loop detectionwith RelocalizationStates from Loop ClosureFealure retrievel oop Deleted二二1---11------22===Global Pose Graph4-DoF Pose Graph OptimizationKeyframe DatabaseOptimization图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 windowIMU:k States from loop clos1Camera:冷 MU measurements>visual measurements★ Catur图2滑窗优化示意图14闭环检测和优化利用D)BoW进行闭环检测,当检测成功后进行重定位,最后对整个相机轨迹进行闭环优化。U预积分VisionIMUVision图3MU预积分示意图21当前时刻pVQ的连续形式将第k唢和第kl帧之间的所有IMU进行积分,可得第kHI帧的位置、速度和旋转(PVQ),作为视觉估计的初始值,这里的旋转采用的四元数。v△t+k+1∈[k,k+1]rW(at-ba ) -owletbk JtE[k, k+1]n(,-bdt∈[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)ka,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△)+ak+1b其中DtElk, k+1t∈[k,k+1R k(at-bar)ldt)Wendtt∈[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+1sb24两帧之间PVQ增量的欧拉法离散形式面给出离散时刻的IMU预积分公式,首先按照论文中采用的欧拉法,给出第i个MU时刻与第i1个IMU时刻的变量关系为b+1k+的6t+元R(P)(1+R(P")(a2-bbn)δt25两帧之间PⅤQ增量的中值法离散形式卜面给出代码中采用的基」中值法的IMU预积分公式,这与 Estimator: processIMUO函数中的 Integration Base: push backo上是一致的。注意这里跟公式(2)是不一样的,这里积分出来的是前后两顿之间的IU增量信息,而公式(2)给出的当前帧时刻的物理量信息+1+B k St +=a, &tbb+1Bi + au1其中a,=slqilai-bai)+qiDi t aitl2.6连续形式下PVQ增量的误差、协方差及 JacobianIMU在每个吋刻积分出来的值是有误差的,下面我们对误差进行分析。首先我们直接给出在t时刻误差项的导数为:sa00016a000000-82(066hkR;0006|=00-(a-bh)0-1192k|+|000|mLL000016ba00101n000018b000F+ozk+ Gt其中:F25×15,G215×2,62x1,n12×,上式推导可参考附录102。下面我们讨论它的作用,将其可以简写为:6之k=F62z+Gtnt根据导数定义可知:62b=1m24-6262+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为表示噪声项的对角协方差矩阵000003000另外根据(11)式可获得诀差项的 Jacobian的迭代公式:(I+F26t)(14)其中 Jacobian的初始值为bk=12.7离散形式的PVQ增量误差分析我们首先直接给出PVQ增量误差在离散形式下的矩阵形式,为了与代码一致,我们修改下变量顺序,这和代码中 midPointIntegration(函数是一致的。(但不知为何计算的V中与前四个噪声项相关的差个负号?)1t fo660f106t‖loeBk+1=0f211f20016bδb0[6b102001rnot000kRkotk+1(15006t0n0000δt其中,推导可参考附录10.3:stE(ak-ba)02-4B+1(kk+121k+1b.)6t|6t2(Rr+ rk+18t2Stn=71=Rk+1(a+1-b)6tWr+ wf1=Ik+11+Gb。)δt-Rn+1(ak+121=-2配+1Stl st21(RK+Ruts)4rula1RrotstStR+1(a1R,+114/+11t28离散形式的PVQ增量误差的 Jacobian和协方差将公式(15)简写为:k+1F15×158215×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为表示噪声项的对角协方差矩阵:00000000aa000Q18×180a00(18)000000三、后端非线性优化31状态向量状态向量共包括滑动窗口内的n+l1个所有相机的状态(包括位置、朝向、速度、加速度计bias和陀螺仪bias)、 Camera到IMU的外参、m+1个3D点的逆深度X=[xr=pw,vb bpc,q3.2目标函数吗+(喻,2)+2(19)其中三个残差项即误差项分别为边缘化的先验信息、IMU测量残差、视觉的重投影残差。三种残差都是用马氏距离表示。根据《十四讲》中高斯牛顿法,若要计算目标函数的最小值,可以理解为,当优化变量有一个增量后,目标函数值最小,以IU残差为例,可写成如下所示:nin lre2bk, X+8Xrk x)+HSⅩDk+1oXk+1k+1其中HB,为B关于 XIK Jacobian,将上式展开并令关于6X的导数为0,可得增量δx的计算公式:H k 8X=k+1TB那么,公式(28)可写成+∑+∑Tk∑1rc上式中,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+1bk qbk+1bR+1 xyz+g"△t)-Bk(20)sbbbb其中各增量关于bias的 Jacobian可从公式(16)的大 Jacobian中的相应位置获得。上面与代码中 Integration base: evaluateD对应,2)优化变量pb, 0W, Svb ,8ba:,bor Opb,, 80W ,Swb,, bakr, Sba3)Jacobian:计算 Jacobian时,残差对应求偏导对象分别为p6e,6vB,6h,ba],6b,6b
- 2020-12-07下载
- 积分：1
MIKE21教程
不错的MIKE21中文教程，主要介绍MIKE21水动力模块方面的内容Www.Zlvo.Com42.7风场( Wind forcing)···;;;·36注意:42.8冰盖( ce coverage)4.2.9引潮势( Tidal potential)42.10降水-蒸发( Precipitation- Evaporation)….4142.1波浪辐射应力( Wave radiation)424212源( Sources4342.13水工结构物( Structures)454.2.14初始条件( nitial conditions)42.15边界条件( Boundary conditions)6142.16温度/盐度模块(Tcmpcraturc/Salinity Module)6742.17湍流模块( Turbulence module)42.8解耜( Decoupling)….…6742.9输出( Outputs)…特别说明:本手册部分内容来源于网络。Www.Zlvo.Com第一章模型介绍11简介MIKE21是一个专业的工程软件包,用于模拟河流、湖泊、河口、海湾、海岸及海洋的水流、波浪、泥沙及环境。MIKE21为工程应用、海岸管理及规划提供了完备、有效的设计环境。高级图形川户界面与高效的计算引擎的结合使得MIKE2I在世界范围内成为了一个水流模拟专业技术人员不可缺少的工具。丹麦水力研究所开发的平面二维数学模型MIKE21,曾经在丹麦、埃及、澳洲、泰国及中国香港、台湾等国家和地区得到成功应用,在丬面二维白由表面流数值模拟方面具有强大的功能。目前该软件在中国的应用发展很快,并在一些大型工程中广泛应用,如:长江口综合治理工程、杭州湾数值模拟、南水北调工程、重庆市城市排污评价、太湖富营养模型、香港新机场工程建设等。12MIKE21软件特点(1)用户界面友好,属于集成的 Windows图形界面;(2)具有强大的前、后攵理功能。在前处理方面,能根据地形瓷料进行计算网格的划分;在后处理方面具有强大的分析功能,如流场动态演示及动画制作、计算断面流量、实测与计算过程的验证、不同方案的比较等;(3)多种计算网格、模块及许可选择确俫用户根据自身需求来选择模型(4)可以进行热启动,当用户因各种原因需暂时中断MIKE21模型时,只要在上次计算时设置了热启动文件,再次开始计算时将热启动文件调入便可继续计算,极大地方便了计算时间有限制的用户;(5)能进行干、湿节点和干、湿单元的设置,能较方便地进行滩地水流的模拟:(6)具有功能强大的卡片设置功能,可以进行多种控制性结构的设置,如桥墩、堰、闸、涵洞等(7)可广泛地应用于二维水力学现象的研究,潮汐、水流,风暴潮,传热、盐流,水质,波浪紊动,湖震,防浪堤布置,船运,泥沙侵蚀、输移和沉积等,Www.Zlvo.Com被推荐为河流、湖泊、河∏和海岸水流的二维仿真模拟工具。1.3水动力模块原理131控制方程模型是基于三向不可压缩和 Reynolds值均布的 Navier-SLokes方程,并服从于 Boussinesq假定和静水压力的假定。二维非恒定浅水方程组为Ch Chu chvhSChu ahauvan h6x+=1-a=欧h-a(1-22pa ax po po ph)+-(h12)+hu,Schv chuy chvfuh-ghan h apay po aygh ap2 Po ay Po po po、ax11)+hS式中:t为时间:x,y为笛卡尔坐标系坐标;n为水位;d为静止水深;h=n+d为总水深;tn,v分别为x,y方向上的速度分量;f是哥氏力系数,f=2 osin p,(为地球白转角速度,为当地纬度;g为重力加速度;p为水的密度;Sx、SS分别为辐射应力分量;S为源项;(uy,ν)为源项水流流速。字母上带横杠的是平均值。例如,矿、ν为沿水深平均的流速,由以下公式定义hu= udz, hvdzWww.Zlvo.Com为水平粘滞应力项,包括粘性力、紊流应力和水平对流,这些量是根据沿水深平均的速度梯度用涡流粘性方程得出的:T=2A2A13,2数值解法)空间离散计算区域的空间离散是用有限体积法( Finite volume method),将该连续统体细分为不重叠的单元,单元可以是任意形状的多边形,但在这里只考虑三角形和四边形单元。在MKE软件2007版本只能是三角形网格。浅水方程组的通用形式一般可以写成上(U)=S(U)(1-6)式中:U为守恒型物理向量:F为通量向量;S为源项在笛卡尔坐标系中,二维浅水方程组可以写为OU O(F-F)O(FY-Fy)S(1-7)式中:上标/和分别为无粘性的和粘性的通量。各项分别如下:0hCu+g(FhuyOu Cha0Fk=lhAolhugh42Www.Zlvo.Comadh2thPu cy Pogn+fuhpe gn opythiPo oy Po对方程(46)第i个单元积分,并运用 Gauss原理重写可得出「a(Fa)-JA(1-9)式中:A1为单元g2的面积;I;为单元的边界;ds为沿着边界的积分变量这里使用单太求积法来计算面积的积分,该求积点位于单元的质点,同时使用中点求积法水计算边界积分,方程(49)可以写为∑FnAT=S(1-10)式中:U和S分别为第个单元的U和S的平均值,并位于单元中心;NS是单元的边界数;^厂,为第j个单元的长度阶解法和二阶解法都可以用于空间离散求解。对于二维的情况,近似的Riemann解法可以用来计算单元界面的对流流动。使用Roc方法时,界面左边的和右边的相关变量需要估计取值。二阶方法中,空间准確度可以通过使用线性梯度重构的技术来获得。而平均梯度可以用由 jawahar和 Kamath于2000年提出的方法来估计,为了避免数值振荡,模型使用了二阶TVD格式。(2)时间积分考虑方程的一般形式aU=G(U)1-11)对于二维模拟,浅水方程的求解有两种方法:一种是低阶方法,另一种是高阶方法。低价方法即低阶显式的Euer方法Un1=Un+△G(Un)(1-12)式中:为时间步长。高阶的方法为以如下形式的使用了二阶的 Runge kuttaWww.Zlvo.Com方法n12=Un+△G(U,)Un+1=Un+△G(Un+12)(1-13)(3)边界条件1)闭合边界沿着闭合边界(陆地边界),所有垂直于边界流动的变量必须为0。对于动量方程,可以得知沿着陆地边界是完全平稳的。2)开边界开边界条件可以指定为流量过程或者是水位过程3)千湿边界处理动边界问题(T湿边界)的方法是基于赵棣华(1994)和 Sleigh(1998)的处理方式。当深度较小时,该问题可以被重新表述,通过将动量通量设置为零以及只考恳质量通量来实现。只有当深度足够小时,计算不考虑该网格屮元。每个单元的水深会被监测,并且单元会被定义为干、半干湿和湿。单元面也会被监测,以确定淹没边界。满足下面两个条件单元边界被定义为淹没边界:首先单元的一边水深必须小于hn,且另一边水深必须大于h;第二,水深小于hn的单元的静水深加上另一单元表面高程水位必须大于零。满足下亩两个条件单元会被定义为干单元:首先单元中的水深必须小于干水深hn;另外,该单元的三个边界中没有一个是淹没边界。被定义为干的单元在计算中会被忽略不计。单儿破定义为半干:如果单元水深介于h和hm之间,或是当水深小于hy但有一个边界是淹没边界。此时动量通量被设定为0,只有质量通量会被计算。单元会被定义为湿:如果单元水深大于ha。此时动量通量和质量通量都会在计算中被考虑。如果模型中的区域是处在τ湿边交替区,为了避免模型计算岀现不稳定性,使用者可以启用 Flood and Dry选项。在这个情形下使用者必须设定一个干水深Www.Zlvo.Com( drying depth),淹没深度( flooding water depth)和湿水深( wetting depth)者必须满足hn
- 2020-12-09下载
- 积分：1
