在线设计工具的网站怎么做,听歌网站源码,关键词智能优化排名,论文引用网站数据 如何做注释用逆深度是因为这样可以在优化中从优化3个变量降低到1个#xff0c;降低优化的维度加快求解速度 用逆深度是因为当距离很远的时候#xff0c; 1 x \frac{1}{x} x1 x x x 就会无穷大#xff0c;而3D点很近的情况也一般不会有#xff0c;这也是为了数值稳定性
用逆深度的…用逆深度是因为这样可以在优化中从优化3个变量降低到1个降低优化的维度加快求解速度 用逆深度是因为当距离很远的时候 1 x \frac{1}{x} x1 x x x 就会无穷大而3D点很近的情况也一般不会有这也是为了数值稳定性
用逆深度的话就要和其中一帧进行绑定这个就是和观测到该点的第一帧进行绑定这样才能表示一个3D点信息
划窗中维护的全部都是IMU下的位姿所以相机要通过外参变换到IMU坐标系下 这里就构成了视觉误差需要求关于优化变量的雅可比矩阵这里约束了第 i i i 帧和第 j j j 帧的 IMU 的姿态同时还会优化相机和IMU的外参这个也是紧耦合的特点之一上一节同时优化 IMU预积分自身的零偏Ba也是紧耦合特点之一3D点逆深度也是要优化总共就是4个参数
转换公式如下 第 i i i 帧归一化坐标系 - 第 j j j 帧相机系 1 λ \frac{1}{\lambda} λ1就是深度 λ \lambda λ 是逆深度 将旋转和平移分开后如下
将刚刚第 i i i 帧相机系下的3D点进行归一化然后和光流追踪到的匹配点进行残差计算这就获得了视觉重投影误差
计算残差对优化量的雅可比
有带时间延时估计的雅可比计算和不带时间估计的雅可比计算 这里先讲不带时间延时的雅可比计算
这里的误差项是2维的坐标点是3维的 要求误差对旋转的雅可比只能通过链式求导的方式 ∂ r ∂ p j ⋅ ∂ p j ∂ x \frac{\partial r}{\partial p_{j}}·\frac{\partial p_{j}}{\partial x} ∂pj∂r⋅∂x∂pj ∂ r ∂ p j \frac{\partial r}{\partial p_{j}} ∂pj∂r 是2×3维的对平移 x , y , z x,y,z x,y,z 进行求导 [ 1 z 0 − x z 2 0 1 z − y z 2 ] \begin{bmatrix}\frac{1}{z}0-\frac{x}{z^{2}} \\ 0\frac{1}{z}-\frac{y}{z^{2}} \end{bmatrix} [z100z1−z2x−z2y]
这里的误差也有协方差矩阵提点的置信度是设定为1.5个像素不变 残差也得乘上置信度
计算 p j p_{j} pj 对 T T T 的雅可比
平移 x , y , z x,y,z x,y,z 的公式为 T T T 包含旋转 R R R 和平移 t t t
对 i i i 时刻求导 i i i 时刻的变量也是要优化的量所以当然也要求导
对 p w b i p_{wb_{i}} pwbi 求导 ∂ p j ∂ p w b i R b c T ⋅ R w b j T \frac{\partial p_{j}}{\partial p_{wb_{i}}}R^{T}_{bc}·R^{T}_{wb_{j}} ∂pwbi∂pjRbcT⋅RwbjT
对 R w b i R_{wb_{i}} Rwbi 求导 ∂ p j ∂ R w b i \frac{\partial p_{j}}{\partial R_{wb_{i}}} ∂Rwbi∂pj 先把公式中有 R w b i R_{wb_{i}} Rwbi 的项提取出来 R b c T R w b j T R w b i ( R b c P c i p b c ) R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}(R_{bc}P^{c_{i}}p_{bc}) RbcTRwbjTRwbi(RbcPcipbc) A R w b i b AR_{wb_{i}}b ARwbib
后面那一串 b b b 乘完后是向量所以可以对李代数进行扰动求导纯旋转矩阵是不能对李代数求导的因为矩阵无法对向量求导这里是乘完后是个向量所以可以用向量来表示旋转的扰动量然后用导数的定义来进行求导 ∂ A R w b i b ∂ ϕ A R w b i e x p ( ϕ ∧ ) b − A R w b i b ϕ \frac{\partial AR_{wb_{i}}b}{\partial \phi}\frac{AR_{wb_{i}}exp(\phi^{\wedge})b-AR_{wb_{i}}b}{\phi} ∂ϕ∂ARwbibϕARwbiexp(ϕ∧)b−ARwbib A R w b i ( I ϕ ∧ ) b − A R w b i b ϕ \frac{AR_{wb_{i}}(I\phi^{\wedge})b-AR_{wb_{i}}b}{\phi} ϕARwbi(Iϕ∧)b−ARwbib A R w b i ϕ ∧ b ϕ \frac{AR_{wb_{i}}\phi^{\wedge}b}{\phi} ϕARwbiϕ∧b 根据反对成矩阵的性质 − A R w b i b ∧ ϕ ϕ \frac{-AR_{wb_{i}}b^{\wedge}\phi}{\phi} ϕ−ARwbib∧ϕ − A R w b i b ∧ -AR_{wb_{i}}b^{\wedge} −ARwbib∧
那个信息矩阵乘完第一步也得乘进来这里这个第2步的雅可比矩阵
对 j j j 时刻进行求导
对 p w b j p_{wb_{j}} pwbj 求导 ∂ p j ∂ p w b j − R b c T ⋅ R w b j T \frac{\partial p_{j}}{\partial p_{wb_{j}}}-R^{T}_{bc}·R^{T}_{wb_{j}} ∂pwbj∂pj−RbcT⋅RwbjT
对 R w b j R_{wb_{j}} Rwbj 求导
把和 R w b j R_{wb_{j}} Rwbj 有关的项提取出来 R b c T R w b j T ( R w b i R b c P c i R w b i p b c p w b i − p w b j ) R^{T}_{bc}R^{T}_{wb_{j}}(R_{wb_{i}}R_{bc}P^{c_{i}}R_{wb_{i}}p_{bc}p_{wb_{i}}-p_{wb_{j}}) RbcTRwbjT(RwbiRbcPciRwbipbcpwbi−pwbj) A R w b j T b AR^{T}_{wb_{j}}b ARwbjTb
对 R w b j T R^{T}_{wb_{j}} RwbjT 是只能左乘的但是我们现在要算他右乘的扰动方向因为方向会不同用左乘的求导结果加个负号就是右乘的结果这里推导直接用右乘所以要加个逆把这个转置消掉来进行推导 A ( R w b j e x p ( ϕ ∧ ) ) − 1 b − A ( R w b j ) − 1 b A(R_{wb_{j}}exp(\phi^{\wedge}))^{-1}b-A(R_{wb_{j}})^{-1}b A(Rwbjexp(ϕ∧))−1b−A(Rwbj)−1b A ( I − ϕ ∧ ) R w b j T b − A ( R w b j ) − 1 b A(I-\phi^{\wedge})R^{T}_{wb_{j}}b-A(R_{wb_{j}})^{-1}b A(I−ϕ∧)RwbjTb−A(Rwbj)−1b − A ϕ ∧ R w b j T b -A\phi^{\wedge}R^{T}_{wb_{j}}b −Aϕ∧RwbjTb A ( R w b j T b ) ∧ ϕ A(R^{T}_{wb_{j}}b)^{\wedge}\phi A(RwbjTb)∧ϕ 消去 ϕ \phi ϕ A ( R w b j T b ) ∧ A(R^{T}_{wb_{j}}b)^{\wedge} A(RwbjTb)∧
后面的 R w b j T b R^{T}_{wb_{j}}b RwbjTb 实际就是 3D 点在 第 j j j 帧 IMU系下的位姿按照展开前的刚体变换来理解一下就好了
对 IMU-相机 的外参求导$
对 p b c p_{bc} pbc 求导 R b c T R w b j T R w b i − R b c T R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}-R^{T}_{bc} RbcTRwbjTRwbi−RbcT
对 R b c R_{bc} Rbc 求导
代码中的 r i c R b c , t i c t b c ricR_{bc},tict_{bc} ricRbc,tictbc Q R w b QR_{wb} QRwb
导数是符合加法的 ( f ( x ) g ( x ) ) ′ f ′ ( x ) g ′ ( x ) (f(x)g(x))^{}f^{}(x)g^{}(x) (f(x)g(x))′f′(x)g′(x)
加法后面的求导结果 ( R b c T b ) (R^{T}_{bc}b) (RbcTb) 这个推导和上面类似就不详细写了
加法前面的求导稍微复杂一点 ( R b c e x p ( ϕ ∧ ) ) − 1 R w b j T R w b i R b c T e x p ( ϕ ∧ ) P c i − R b c T R w b j T R w b i R b c T P c i (R_{bc}exp(\phi^{\wedge}))^{-1}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}exp(\phi^{\wedge})P^{c_{i}}-R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}P^{c_{i}} (Rbcexp(ϕ∧))−1RwbjTRwbiRbcTexp(ϕ∧)Pci−RbcTRwbjTRwbiRbcTPci
下面暂时省略写后面它自身 ( I − ϕ ∧ ) R b c T R w b j T R w b i R b c T ( I ϕ ∧ ) P c i (I-\phi^{\wedge})R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}(I\phi^{\wedge})P^{c_{i}} (I−ϕ∧)RbcTRwbjTRwbiRbcT(Iϕ∧)Pci ( I − ϕ ∧ ) A ( I ϕ ∧ ) P c i (I-\phi^{\wedge})A(I\phi^{\wedge})P^{c_{i}} (I−ϕ∧)A(Iϕ∧)Pci ( A − ϕ ∧ A ) ( I ϕ ∧ ) P c i (A-\phi^{\wedge}A)(I\phi^{\wedge})P^{c_{i}} (A−ϕ∧A)(Iϕ∧)Pci ( A A ϕ ∧ − ϕ ∧ A − ϕ ∧ A ϕ ∧ ) P c i − A P c i (AA\phi^{\wedge}-\phi^{\wedge}A-\phi^{\wedge}A \phi^{\wedge})P^{c_{i}}-AP^{c_{i}} (AAϕ∧−ϕ∧A−ϕ∧Aϕ∧)Pci−APci
其中 ϕ ∧ A ϕ ∧ \phi^{\wedge}A \phi^{\wedge} ϕ∧Aϕ∧ 是二阶是相对于一阶的无穷小这里只讨论一阶的展开且 ϕ \phi ϕ 本身就是小量所以直接约掉 ( A ϕ ∧ − ϕ ∧ A ) ⋅ P c i (A\phi^{\wedge}-\phi^{\wedge}A)·P^{c_{i}} (Aϕ∧−ϕ∧A)⋅Pci − A P ∧ ϕ ( A P ) ∧ ϕ -AP^{\wedge}\phi(AP)^{\wedge}\phi −AP∧ϕ(AP)∧ϕ 约掉 ϕ \phi ϕ − A P ∧ ( A P ) ∧ -AP^{\wedge}(AP)^{\wedge} −AP∧(AP)∧
对逆深度 λ \lambda λ 求导 ∂ p j ∂ P c i ∂ P c i ∂ λ \frac{\partial p_{j}}{\partial P^{c_{i}}}\frac{\partial P^{c_{i}}}{\partial \lambda} ∂Pci∂pj∂λ∂Pci
前面的 ∂ p j ∂ P c i R b c T R w b j T R w b i R b c \frac{\partial p_{j}}{\partial P^{c_{i}}}R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R_{bc} ∂Pci∂pjRbcTRwbjTRwbiRbc P c i 1 λ ⋅ p P^{c_{i}}\frac{1}{\lambda} ·p Pciλ1⋅p p p p 是归一化相机系下的3D点 ∂ P c i ∂ λ − 1 λ 2 ⋅ p \frac{\partial P^{c_{i}}}{\partial \lambda}-\frac{1}{\lambda^{2}}·p ∂λ∂Pci−λ21⋅p
这个 − 1 λ 2 -\frac{1}{\lambda^{2}} −λ21 是个系数移到哪里都可以
零空间漂移处理
优化的时候会固定滑窗中的第一帧的xyz和yaw角因为IMU约束的是相对位姿且IMU的4个不可观自由度就是 y a w 、 x 、 y 、 z yaw、x、y、z yaw、x、y、z绝对位姿是没有约束的所以可能会产生在 4自由度的 0 空间漂移的情况fusion中的GPS就是约束绝对位姿的。
VINS中的固定是先计算第一帧的yaw和xyz的偏移量然后把后面的帧都减去这个偏移量偏移回之前的位置这样的做法就类似ORB中的固定第一帧的位姿不过这里是减去第一帧的偏移量其实就是滑窗中的整条轨迹调整回偏移前的位置这样就保证不受 0 空间的影响。
和 yaw相关的量都会受影响就是和旋转向量的量都会受影响 P , V P,V P,V 受影响零偏 B a , B g Ba,Bg Ba,Bg 外参 T b c Tbc Tbc 不受影响
前面会把旋转矩阵变成rpy然后把yaw的角度差取出来构成新的偏移旋转矩阵因为只是yaw发生漂移
