做视频网站要什么软件下载,秦皇岛建设路小学网站,找人开发一个网站多少钱,企业网站域名在哪申请整体思想就是根据预积分的公式#xff0c;把已知量和未知量各放到一边#xff0c;因为前面的数据都是变换到 c 0 c_{0} c0下的#xff0c;不是真正意义上和重力对齐的世界坐标#xff0c;然后位移和速度的预积分中会用到加速度计获取的重力加速度g#xff0c;但是这个重…整体思想就是根据预积分的公式把已知量和未知量各放到一边因为前面的数据都是变换到 c 0 c_{0} c0下的不是真正意义上和重力对齐的世界坐标然后位移和速度的预积分中会用到加速度计获取的重力加速度g但是这个重力g是没有和世界的重力方向对齐的所以里面就存在了未知量通过每组预积分的数据联立起来构成一个大矩阵对重力向量进行求解这里是求解了重力方向对于起始帧 c 0 c_{0} c0的重力方向重力大小也有不过不准确所以后续还会利用重力先验对方向进行调整
有个小问题这个预积分前面不是也会涉及重力吗是直接不理会当作影响不大来进行处理那这个已知量里面也包含里一定的未知量的积分影响在里面待我看下代码确定一下整体的初始化流程和思想后续会出篇整体初始化思想流程
关于位移的预积分量的定义把 c 0 c_{0} c0 换成 w w w 就是之前的式子了因为重力还没对齐所以只转换到 c 0 c_{0} c0 坐标系下就是系统开始的第一帧下
先对平移进行推导 上一节零偏估计里面推导出这个式子 代入进去则有 R c 0 b k ( s P c k 1 c 0 − R b k 1 c 0 ⋅ P c b ) R c 0 b k [ s P c k c 0 − R b k c 0 ⋅ P c b R c 0 b k ⋅ v b k ⋅ Δ t − 1 2 g c 0 Δ t 2 α ] R^{b_{k}}_{c_{0}}(sP^{c_{0}}_{c_{k1}}-R^{c_{0}}_{b_{k1}}·P^{b}_{c})R^{b_{k}}_{c_{0}}[sP^{c_{0}}_{c_{k}}-R^{c_{0}}_{b_{k}}·P^{b}_{c}R^{b_{k}}_{c_{0}}·v^{b_{k}}·Δt-\frac{1}{2}g^{c_{0}}Δt^{2}α] Rc0bk(sPck1c0−Rbk1c0⋅Pcb)Rc0bk[sPckc0−Rbkc0⋅PcbRc0bk⋅vbk⋅Δt−21gc0Δt2α] 原本的 v b k c 0 v^{c_{0}}_{b_{k}} vbkc0 指的是再 c 0 c_{0} c0 坐标系下 b k b_{k} bk 的速度现在直接拿在 b k b_{k} bk 系下的速度通过旋转矩阵进行变换其实意思是一样的。 现在把已知量放左边未知量放右边 α − P c b ⋅ R c 0 b k ⋅ R b k 1 c 0 ⋅ P c b R c 0 b k ( P c k 1 c 0 − P c k c 0 ) s − v b k Δ t 1 2 R c 0 b k g c 0 Δ t 2 α-P^{b}_{c}·R^{b_{k}}_{c_{0}}·R^{c_{0}}_{b_{k1}}·P^{b}_{c}R^{b_{k}}_{c_{0}}(P^{c_{0}}_{c_{k1}}-P^{c_{0}}_{c_{k}})s-v^{b_{k}}Δt\frac{1}{2}R^{b_{k}}_{c_{0}}g^{c_{0}}Δt^{2} α−Pcb⋅Rc0bk⋅Rbk1c0⋅PcbRc0bk(Pck1c0−Pckc0)s−vbkΔt21Rc0bkgc0Δt2 左边都是已知量最后是一个 3 × 1 3×1 3×1 的一个向量 上面的式子就和论文中的公式对应了
对速度进行推导
根据原始预积分的定义有 R c 0 b k ⋅ V b k 1 c 0 R c 0 b k ( V b k c 0 − g c 0 ⋅ Δ t ) β R^{b_{k}}_{c_{0}}·V^{c_{0}}_{b_{k1}}R^{b_{k}}_{c_{0}}(V^{c_{0}}_{b_{k}}-g^{c_{0}}·Δt)β Rc0bk⋅Vbk1c0Rc0bk(Vbkc0−gc0⋅Δt)β 然后变换一下表达方式含义是不变的 R c 0 b k ⋅ R b k 1 c 0 ⋅ V b k 1 R c 0 b k ( R b k c 0 ⋅ V b k − g c 0 ⋅ Δ t ) β R^{b_{k}}_{c_{0}}·R^{c_{0}}_{b_{k1}}·V^{b_{k1}}R^{b_{k}}_{c_{0}}(R^{c_{0}}_{b_{k}}·V^{b_{k}}-g^{c_{0}}·Δt)β Rc0bk⋅Rbk1c0⋅Vbk1Rc0bk(Rbkc0⋅Vbk−gc0⋅Δt)β 同样把已知量放左边未知量放右边 β − V b k R c 0 b k ⋅ R b k 1 c 0 ⋅ V b k 1 R c 0 b k ⋅ g c 0 ⋅ Δ t β-V^{b_{k}}R^{b_{k}}_{c_{0}}·R^{c_{0}}_{b_{k1}}·V^{b_{k1}}R^{b_{k}}_{c_{0}}·g^{c_{0}}·Δt β−VbkRc0bk⋅Rbk1c0⋅Vbk1Rc0bk⋅gc0⋅Δt
第 k 到 k1 都会产生上面两个式子会有很多个这种式子拼起来
构成 H x b Hxb Hxb 的矩阵方程代码中就是一些矩阵拼接的操作 x [ v k , v k 1 , g , s ] ∈ 10 × 1 x[v_{k},v_{k1},g,s]∈10×1 x[vk,vk1,g,s]∈10×1所以 H 属于 10 × 10 H属于10×10 H属于10×10 b ∈ 10 × 1 b∈10×1 b∈10×1实际上由于有多组数据10 会变成 N速度重力都是3维的尺度因子是1所以每一组就是 10
有多个数据则矩阵写为通过对每一组数据进行矩阵分块填入这个大矩阵里面然后一次性求解出所有速度、重力和尺度
求解后获得重力向量 然后根据当地的重力先验来对刚才算出来的重力进行修正
