【VSLAM系列】二:VIO相关原理笔记

VIO(Visual-Inertial Odometry)

第一章 概述

1.1 IMU特点

IMU(Inertial Measurement Unit),惯性测量单元,由一个陀螺仪和加速度计组成
优势:快速响应,角速度普遍比较准确,课估计绝对尺度
劣势:存在零偏,低精度IMU积分姿态发散,高精度价格昂贵

IMU与视觉的互补性质:
IMU适合计算短时间、快速的运动
视觉适合计算长时间、慢速的运动
同时可利用视觉定位信息来估计IMU的零偏,减少IMU由零偏导致的发散和累积误差,反之,IMU可以为视觉提供快速运动的定位
松耦合:IMU与视觉各自运算,后处理的方式进行融合,卡尔曼滤波,融合过程对二者本身不影响
紧耦合:IMU与视觉共同参与位姿估计,融合的过程会影响视觉与IMU的参数MSCKF和非线性优化 ,一次性建模所有的运动和测量信息

1.2 Eigen库相关

1、旋转向量

//初始化旋转向量:旋转角为alpha,旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z));

// 旋转向量转[旋转矩阵](https://so.csdn.net/so/search?q=旋转矩阵&spm=1001.2101.3001.7020)
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.[matrix];
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.toRotationMatrix();

//旋转向量转[欧拉角](https://so.csdn.net/so/search?q=欧拉角&spm=1001.2101.3001.7020)(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

//旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;Quaterniond quaternion;
Eigen::Quaterniond quaternion;quaternion=rotation_vector;

2、旋转矩阵

//初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

// 旋转矩阵转旋转向量
Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;rotation_vector=rotation_matrix;
Eigen::AngleAxisd rotation_vector;rotation_vector.fromRotationMatrix(rotation_matrix);

//旋转矩阵转欧拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

//旋转矩阵转四元数
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;quaternion=rotation_matrix;

3、欧拉角

//初始化欧拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle(yaw,pitch,roll);

//欧拉角转旋转向量
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

// 欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::Matrix3d rotation_matrix;rotation_matrix=yawAngle*pitchAngle*rollAngle;

// 欧拉角转四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::Quaterniond quaternion;quaternion=yawAngle*pitchAngle*rollAngle;

4、四元数

//初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);

//四元数转旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

//四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.toRotationMatrix();

// 四元数转欧拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

1.3 相关公式推导

使用旋转矩阵 R 时,角速度为 ω,那么 R 相对于时间的导数可写作:

R ˙ = R ω ∧ Ṙ = Rω^∧ R˙=Rω
左扰动雅克比

∂ ( R p ) ∂ φ = − ( R p ) ∧ {∂ (Rp)\over∂φ }=−(Rp)^ ∧ φ(Rp)=(Rp)
右扰动雅克比

∂ ( R p ) ∂ φ = − R p ∧ {∂ (Rp)\over∂φ }=−Rp^ ∧ φ(Rp)=Rp
旋转连乘的雅可比:
d l n ( R 1 R 2 ) d R 2 = J r − 1 ( l n ( R 1 R 2 ) ∨ ) {d ln(R_1R_2)\over d R_2}= J r^{-1} (ln(R_1 R_2 )^∨) dR2dln(R1R2)=Jr1(ln(R1R2))
旋转连乘的雅可比:

d l n ( R 1 R 2 ) d R 1 = J r − 1 ( l n ( R 1 R 2 ) ∨ ) R 2 T {d ln(R_1R_2)\over d R_1}= J r^{-1} (ln(R_1 R_2 )^∨)R_2^T dR1dln(R1R2)=Jr1(ln(R1R2))R2T
对R的逆求导
∂ ( R − 1 p ) ∂ φ = ( R p ) ∧ {∂ (R^{-1}p)\over∂φ }=(Rp)^ ∧ φ(R1p)=(Rp)

d l n ( R 1 R 2 − 1 ) d R 2 = − J r − 1 ( l n ( R 1 R 2 − 1 ) ∨ ) R 2 {d ln(R_1R_2^{-1})\over d R_2}= -J r^{-1} (ln(R_1 R_2 ^{-1})^∨)R_2 dR2dln(R1R21)=Jr1(ln(R1R21))R2

第二章 IMU模型

2.1 旋转运动学

旋转的几种表示方法
(1)旋转矩阵:使用9个量描述3自由度旋转,运算方便,但是会有额外约束(单位正交矩阵),求导困难
(2)旋转向量:经凑的表示形式。但是具有周期性(但是微小变化时可以使用)
(3)欧拉角:直观,用户友好,但是需要指定旋转顺序,存在万向节锁死问题,无法进行球面平滑差值
(4)四元数:紧凑的不带其一的表示方法,对用户不直观,要求党委四元数,表示旋转也会过参数化

粒子在坐标系中 z = h 中的平面做圆周运动,坐标为:
r = ( a c o s θ , a s i n θ , h ) ⊤ r = (a cos θ, a sin θ, h)^ ⊤ r=(acosθ,asinθ,h)
对坐标求导得:
r ˙ = ω × r ṙ = ω × r r˙=ω×r
其中
ω = d ( θ ˙ ) z ω = d(θ̇)z ω=d(θ˙)z
| θ̇| 是角速度大小。
对公式 (1) 两边取模得:
∣ r ˙ ∣ = ∣ ω ∣ ∣ r ∣ s i n φ = a ∣ d ( θ ˙ ) ∣ |ṙ| = |ω||r| sin φ = a |d(θ̇)| r˙=ω∣∣rsinφ=ad(θ˙)

质量块在 body 坐标系下的坐标为:
r b = ( x 1 , x 2 , x 3 ) ⊤ r^b = (x_1 , x_2 , x_3 )^⊤ rb=(x1,x2,x3)
忽略平移,只考虑旋转,则旋转到惯性系
下有:
r i ( t ) = R i b r b r^ i (t) = R_{ib} r^b ri(t)=Ribrb
I系下的速度为对时间求导有:
r ˙ i = v i = R i b v b + ω × r i ṙ^i=v^i = R_{ib} v^b + ω × r^i r˙i=vi=Ribvb+ω×ri
I系下的加速度为对速度求导得到:
r ¨ = a = a i − 2 ω × v − ω ˙ × r i − ω × ( ω × r i ) r̈=a = a ^i − 2ω × v − ω̇ × r^i − ω × (ω × r^i ) r¨=a=ai2ω×vω˙×riω×(ω×ri)
科氏力 欧拉力 离心力

2.2 IMU测量模型与运动学模型

MEMS 加速度计利用电容或者电阻桥来等原理来测量 a_m
加速度计测量值: a m = a − g 加速度计测量值: a_m = a − g 加速度计测量值:am=ag
陀螺仪主要用来测量物体的旋转角速度,按测量原理分有振动陀螺,光纤陀螺等。低端 MEMS 陀螺上一般采用振动陀螺原理,通过测量 Coriolis force 来间接得到角速度

2.3 IMU误差模型

加速度计和陀螺仪的误差可以分为:确定性误差,随机误差
• 确定性误差可以事先标定确定,包括:bias, scale …
• 随机误差通常假设噪声服从高斯分布,包括:高斯白噪声,bias 随机游走…

确定性误差:

Bias每次启动都不一样,运行过程中可能也会变化
理论上,当没有外部作用时,IMU 传感器的输出应该为 0。但是,实际数据存在一个偏置 b。加速度计 bias 对位姿估计的影响:
v e r r = b a t , p e r r = 0.5 b a t 2 v_{err} = b^a t, p_{err} = 0.5b^a t^2 verr=bat,perr=0.5bat2
scale 可以看成是实际数值和传感器输出值之间的比值。

xyz 轴可能不垂直

六面法标定加速度 bias 和 scale factor
六面法是指将加速度计的 3 个轴分别朝上或者朝下水平放置一段时间,采集 6 个面的数据完成标定。

六面法标定陀螺仪,陀螺仪的真实值由高精度转台提供,这里的 6 面是指各个轴顺时针和逆时针旋转

温度标定 soak method: 控制恒温室的温度值,然后读取传感器数值进行标定。

随机误差

高斯白噪声 :IMU 数据连续时间上受到一个均值为 0,方差为 σ,各时刻之间相互独立的高斯过程 n(t):

Bias 随机游走: 常用维纳过程 (wiener process) 来建模 bias 随时间连续变化的过程,离散时间下称之为随机游走。
b ˙ ( t ) = n b ( t ) = σ b w ( t ) ḃ(t) = n_b (t) = σ_b w(t) b˙(t)=nb(t)=σbw(t)
Bias的倒数是高斯噪声 其中 w 是方差为 1 的白噪声

艾伦方差标定

第三章 基于优化的IMU与视觉信息融合

3.1 基于BA的VIO融合

视觉SLAM里面的BA问题

已知
• 状态量初始值:特征点的三维坐标,相机的位姿。
• 系统测量值:特征点在不同图像上的图像坐标。

解决方式
构建误差函数,利用最小二乘得到状态量的最优估计
a r g m i n ( q , p , f ) ∑ i = 1 m ∑ j = 1 n ∣ ∣ π ( q w c i , p w c i , f j ) − z f j c i ∣ ∣ Σ i j arg min (q,p,f)∑^m_{i=1}∑^n _{j=1} ||π (q_{wc_i} , p_{wc _i} , f_j ) −z^{c_i}_{f_j}||Σ_{ij} argmin(q,p,f)i=1mj=1n∣∣π(qwci,pwci,fj)zfjci∣∣Σij
q:旋转四元数, • p:平移向量 • f :特征点 3D 坐标 • c i :第 i 个相机系 • π(·):投影函数 • z c f i :c i 对 f j 的观测
j • Σ ij :Σ 范数

备注:四元数乘法,可以转化为一个矩阵乘一个四元素[4X4] [4X1] = [4X1]

3.2 最小二乘问题迭代法求解

找一个下降方向使损失函数随 x 的迭代逐渐减小,直到 x 收敛到 x ∗ :F (x k+1 ) < F (x k )
分两步:第一,找下降方向单位向量 d,第二,确定下降步长 α.

1)最速下降法: 适用于迭代的开始阶段
梯度的负方向为最速下降方向
∆ x = − J ∣ ∣ J ∣ ∣ ∆x = {−J\over ||J||} x=∣∣J∣∣J
2)牛顿法:适用于最优值附近
在局部最优点 x ∗ 附近,如果 x + ∆x 是最优解,则损失函数对 ∆x 的导数等于 0,
H − 1 ∆ x = − J ⊤ H^{−1}∆x = − J ^⊤ H1x=J
3)阻尼法,
( H + μ I ) ∆ x = − J ⊤ (H + μI) ∆x = −J^⊤ (H+μI)x=J
4)高斯牛顿法
( J ⊤ J )∆ x g n = − J ⊤ f = b (J^⊤ J )∆x_{gn} = −J^⊤ f=b (JJxgn=Jf=b

5)列文伯格-马夸尔特法LM
( J ⊤ J + μ I ) ∆ x l m = − J ⊤ f = b (J^⊤J+ μI) ∆x_{lm} = −J^⊤ f=b (JJ+μI)xlm=Jf=b

JTJ信息矩阵

鲁邦核函数,M估计,自适应估计

3.3 VIO残差函数的构建

r = ∥ f ( x ) ∥ 2 Σ = f ⊤ ( x ) Σ − 1 f ( x ) r = ∥f (x)∥^2 Σ = f^⊤ (x) Σ^{−1} f (x) r=f(x)2Σ=f(x)Σ1f(x)

其中*,f (x)* 服从高斯分布,协方差为 Σ。 Σ可以将各残差项归一化,并给个权重深度参数化

重投影误差:
请添加图片描述

IMU的残差

IMU PVQ 对时间的导数可写成:
p ˙ w b t = v t w v ˙ t w = a t w q ˙ w b t = q w b t ⊗ [ 0 , 1 2 ω b t ] T p˙_{wbt} = v^w_t \\ v˙^w_t = a^w_t\\ q˙_{wbt} = q_{wbt} ⊗ [ 0 ,{1\over2}ω^{bt}]^T p˙wbt=vtwv˙tw=atwq˙wbt=qwbt[0,21ωbt]T
从第 i 时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ:(注意,这里积分项中的姿态是相对于世界坐标系的)
p w b j = p w b i + v i w ∆ t + ∫ ∫ t ∈ [ i , j ] ( q w b t a b t − g w ) δ t 2 v j w = v i w + ∫ t ∈ [ i , j ] ( q w b t a t b − g w ) δ t q w b j = ∫ t ∈ [ i , j ] q w b t ⊗ [ 0 , 1 2 ω b t ] T δ t p_{wbj} = p_{wbi} + v^w_i ∆t + ∫ ∫_{t∈[i,j]} (q_{wb_t} a ^{b_t} − g^ w)δt^2\\ v^w_j = v^w_i + ∫_{t∈[i,j]} (q_{wb_t} a^b_t − g^w)δt \\ q_{wbj }= ∫_{t∈[i,j]} q_{wbt} ⊗ [ 0 ,{1 \over2 }ω^bt ]^T δt pwbj=pwbi+viwt+t[i,j](qwbtabtgw)δt2vjw=viw+t[i,j](qwbtatbgw)δtqwbj=t[i,j]qwbt[0,21ωbt]Tδt
IMU预积分

引入预积分的核心是不要使IMU积分量与优化变量有关

先以上一个时刻的位姿开始积分,然后将积分结果转化到世界坐标系。
预积分量仅仅预IMU的测量值有关,将一段时间内的IMU数据直接积分起来就得到了预积分量。一个很简单的公式转换,就可以将积分模型转为预积分模型:
q w b t = q w b i ⊗ q b i b t q_{wb_t} = q_{wb_i} ⊗ q_{b_ib_t} qwbt=qwbiqbibt
那么,PVQ 积分公式中的积分项则变成相对于第 i 时刻的姿态,而不 是相对于世界坐标系的姿态:(可以将上面的PVQ积分公式中的积分项变成相对于第i时刻的位姿)

预积分量仅仅跟 IMU 测量值有关,它将一段时间内的 IMU 数据直接积分起来就得到了预积分量:
α b i b j = ∫ ∫ t ∈ [ i , j ] ( q b i b t a b t ) δ t 2 β b i b j = ∫ t ∈ [ i , j ] ( q b i b t a b t ) δ t q b i b j = ∫ t ∈ [ i , j ] q b i b t ⊗ [ 0 , 1 2 ω b t ] T δ t α_{bibj} = ∫ ∫_{t∈[i,j] }(q_{b_ib_t}a^{bt} )δt^2\\ β_{bibj} = ∫ _{t∈[i,j] }(q_{b_ib_t}a^ {bt} )δt\\ q_{bibj} = ∫_{ t∈[i,j] }q_{b_ib_t} ⊗ [ 0, {1\over 2}ω^bt ]^T δt αbibj=t[i,j](qbibtabt)δt2βbibj=t[i,j](qbibtabt)δtqbibj=t[i,j]qbibt[0,21ωbt]Tδt

请添加图片描述

IMU预积分误差 定义:一段时间内 IMU 构建的预积分量作为测量值,对两时刻之间的 状态量进行约束,
请添加图片描述

预积分量的方差

误差的传递由两部分组成:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一时刻

假设已知了相邻时刻误差的线性传递方程:
η i k = F k − 1 η i k − 1 + G k − 1 n k − 1 η_{ik} = F_{k−1} η_{ik−1} + G_{k−1 }n_{k−1} ηik=Fk1ηik1+Gk1nk1
协方差矩阵可以通过递推计算得到:
Σ i k = F k − 1 Σ i k − 1 F k − 1 ⊤ + G k − 1 Σ n G k − 1 Σ_{ik} = F_{k−1} Σ_{ik−1} F^⊤ _{k−1} + G_{ k−1} Σ_n G_{k−1} Σik=Fk1Σik1Fk1+Gk1ΣnGk1
用一阶泰勒展开的推导方式推导误差的递推公式: [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-r9svmjEk-1670151619451)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/4.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Tf5jnTfk-1670151619451)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/5.png)]请添加图片描述

3.4 误差卡尔曼滤波

https://www.bilibili.com/read/cv14531305

误差卡尔曼滤波就是以误差值作为滤波状态量. IMU预积分中使用误差卡尔曼滤波来计算置信度,得到误差的协方差矩阵。(主要是因为旋转四元数引起的过参数化问题),误差状态帮助IMU预积分计算协方差矩阵时避免过参数化。

1为什么要用误差卡尔曼滤波?
IMU预积分中使用四元数表示旋转,但是四元数是过参数化的,需要用4x4的矩阵来描述置信度;在误差卡尔曼滤波中,用旋转向量作为参数,这样就可以用3x3的协方差矩阵来衡量旋转的置信度.
误差通常是很小的,接近0,这样就避免了旋转向量的周期性问题.
误差量很小,可以忽略二阶导数的计算,只计算一阶导数更快.
误差变化很慢,用卡尔曼滤波进行状态修正时,可以降低观测频率.

标称状态与真实状态的均值是一致的,误差状态则与真实状态的协方差矩阵一致

误差的状态方程

需要推导离散时间下的预积分项,预积分残差,预积分误差项雅可比矩阵和预积分误差项协方差矩阵,下面为误差的传递方差

雅可比矩阵传播

可以从k时刻单位雅可比矩阵开始通过此式传播,得到i+1时刻的雅可比矩阵,该雅可比矩阵可用于前文提到的零偏变化情况下对预积分进行调整。

协方差矩阵传播

顾线性方程协方差传递的性质
有误差传递方程
首先,最初的协方差矩阵肯定是一个全零矩阵根据协方差传递的性质,有

其中,Q是噪声的协方差矩阵,记为

可以根据一开始的零协方差矩阵开始传播,得到i+1时刻的协方差矩阵。协方差矩阵可用于评判残差项的置信度。

第四章 基于滑动窗口的VIO:可观性与一致性

4.1 高斯分布与信息矩阵

J ⊤ Σ − 1 J δ ξ = − J ⊤ Σ − 1 r J^⊤ Σ ^{−1} Jδξ = −J ^⊤ Σ^{ −1} r JΣ1Jδξ=JΣ1r

segema协方差矩阵 的逆为信息矩阵,可以理解为每个状态的权重

协方差矩阵的逆可以计算联合高斯分布

p ( x 1 , x 2 , x 3 ) = p ( x 2 ) p ( x 1 ∣ x 2 ) p ( x 3 ∣ x 2 ) p(x_1 , x_ 2 , x_ 3 ) = p(x_ 2 )p(x _1 |x _2 )p(x _3 |x_2 ) p(x1,x2,x3)=p(x2)p(x1x2)p(x3x2)
协方差逆矩阵中如果坐标为 (i, j) 的元素为 0,表示元素 i 和 j 关于其他变量条件独立,协方差中非对角元素 Σ ij > 0 表示两变量是正相关。
• 信息矩阵中非对角元素为负数,甚至为 0。Λ 12 < 0 表示在变量x 3 发生的条件下,元素 x 1 和 x 2 正相关。元素大于0表示负相关

4.2 边缘化与舒尔补

舒尔补定义
给定任意的矩阵块 M,如下所示:
M = [ A B C D ] M=[ AB\\ C D ] M=[ABCD]
• 如果,矩阵块 D 是可逆的,则 A − BD ^−1 C 称之为 D 关于 M的舒尔补。
• 如果,矩阵块 A 是可逆的,则 D − CA^ −1 B 称之为 A 关于 M的舒尔补。把矩阵变成上三角或者下三角过程中遇到的角落处的矩阵

**使用舒尔补可以快速求解矩阵的逆 **

基于边际概率的滑动窗口算法直接丢弃变量和对应的测量值,会损失信息,正确的做法是使用边际概率,将被marge的变量所携带的信息传递给剩余变量

单目 + IMU 系统是 4 自由度不可观:yaw 角 + 3 自由度位置不可观。roll 和 pitch 由于重力的存在而可观,尺度因子由于加速度计的存在而可观。

第五章 后端优化

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Q4LpnT8C-1670151619455)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/6.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-pBJTW3M2-1670151619456)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/7.png)]

GTSAM第三方优化库

滑动窗口更新先验残差

• 目的:虽然先验信息矩阵固定不变,但随着迭代的推进,变量被不断优化,先验残差需要跟随变化。否则,求解系统可能奔溃。
• 方法:先验残差的变化可以使用一阶泰勒近似。
b p ′ = b p + ∂ b ∂ x p δ x p = b p + ∂ ( − J ⊤ Σ − 1 r ) ∂ x p δ x p = b p − Λ p δ x p b^′_p = b_p +{∂b \over∂x_p}δx_p\\ = b_p +{∂ (−J^ ⊤ Σ^ −1 r)\over∂x_p}δx_p\\ = b_p − Λ_p δx_p bp=bp+xpbδxp=bp+xp(JΣ1r)δxp=bpΛpδxp

第六章 前端

光流法最早,最成熟,缺点也明显(抗光照干扰弱,依赖角点)

• FAST+ 光流是比较实用的快速算法/GFTT+ 光流效果更好,也具备实时性
• 特征匹配需要提的特征具有旋转平移缩放不变性,SIFT/SURF 是
最好的一类,BRISK 等次之,ORB 算比较差的
• 特征匹配和光流都非常依赖角点,日常生活场景中角点不明显的
很多

后端通常实时性较差,不适合处理所有帧;如果相机停止,可能给后端留下无用的优化,甚至导致后端问题退化

1 关键帧之间不必太近(退化或三角化问题)
2 关键帧之间不能太远(共视点太少)
3 VIO 中,定期选择关键帧(假设 b g , b a 在关键帧期间不变)

对于非关键帧,只执行前端算法,不参与后端优化• 因此,对于非关键帧,它的误差会逐渐累积。直接该帧被作为关键帧插入后端,BA 才会保证窗口内的一致性

第七章 VINS初始化

通过相机与IMU的旋转估计外参数和bias,通过平移约束估计重力向量速度与尺度,但是重力向量的模长是固定的,优化过程中并没有加模长限制,而实际上只有两个自由度,三维向量自由度为2,可以采用球面坐标系参数化

数据集

evo安装与使用

https://www.codenong.com/cs105594262/

git clone https://github.com/MichaelGrupp/evo.git cd evo pip install -i https://pypi.tuna.tsinghua.edu.cn/simple evo --upgrade --no-binary evo

代码讲解

https://blog.csdn.net/weixin_40224537

第八章 时间补偿与同步

8.1 时间戳延迟估计方法

1 基于轨迹匀速模型的时间戳补偿算法

姿态补偿
假设 t 为对齐时刻,延迟量 t d ,延迟后时间戳为 t cam ,
则 t 时刻的相机姿态为
T w c ( t ) = T w b ( t c a m − t d ) ⋅ T b c T_{wc }(t) = T_{ wb} (t^{ cam} − t_ d ) · T_{bc} Twc(t)=Twb(tcamtd)Tbc
具体地,假设相机匀速运动,t cam 时刻 IMU 坐标系速度为 v wb i ,角速度测量值为 ω b i (忽略 bias),补偿公式为
R w b i , − t d ≈ R w b i E x p ( − ω b i t d ) p w b i , − t d ≈ p w b i − v b i w t d R_{ wb _i,−t_ d }≈ R _{wb_ i} Exp (−ω _{b_ i} t _d )\\ p_{ wb_ i ,−t _d }≈ p_{ wb_ i} − v^ w_{b _i} t _d Rwbi,tdRwbiExp(ωbitd)pwbi,tdpwbivbiwtd

补偿后的相机姿态为
R w c i = R w b i , − t d R b c ≈ R w b i E x p ( − ω b i t d ) R b c p w c i = R w b i , − t d p b c + p w b i , − t d ≈ R w b i E x p ( − ω b i t d ) p b c + p w b i − v b i w t d R _{wc _i} = R _{wb _i,−t_ d }R _{bc} ≈ R_{ wb_ i} Exp (−ω_ {b _i} t _d ) R_{bc}\\ p_{wc _i }= R_{wb_ i ,−t _d }p_{bc} + p_{wb_ i ,−t_ d}\\ ≈ R_{wb_ i} Exp (−ω _{b_ i} t _d ) p_{bc }+ p _{wb_ i} − v ^w_ {b_i} t _d Rwci=Rwbi,tdRbcRwbiExp(ωbitd)Rbcpwci=Rwbi,tdpbc+pwbi,tdRwbiExp(ωbitd)pbc+pwbivbiwtd
时间戳延迟变量只对先验残差和视觉重投影误差有影响。IMU 预积分误差中由于不
涉及相机传感器数据,不受时间延迟的影响

基于视觉特征匀速模型的时间戳补偿算法

时间戳延迟使得特征的重投影坐标和真实坐标之间存在误差,可以直接补偿特征坐标 假设视觉特征在图像平面上匀速移动

两种方法的分析
特征匀速模型:计算简单,代码改动小。然而视觉特征在图像平面是非线性运动,特征匀速的假设精度低。
• 轨迹匀速模型:相比于特征匀速,轨迹匀速的假设更接近实际,模型精度高。但雅克比矩阵等计算复杂,代码改动多。

两匀速模型算法对比
3 时间戳同步算法扩展
其他时间戳延迟估计算法
VIO 初始化阶段的时间戳估计

  • 3
    点赞
  • 15
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值