本文已参与「新人创作礼」活动,一起开启掘金创作之路。
参考资料《视觉SLAM十四讲》
首先为什么要引入李群与李代数? 两个坐标系之间的运动由一个旋转加上一个平移组成,这种运动称为刚体运动。对于一个向量
α,假设他在两个坐标系下的坐标分别为
[α1,α2,α3]T和
[α1′,α2′,α3′]T,因为向量本身并没有改变,因此可以得到:
[e1,e2,e3]⎣⎢⎡α1α2α3⎦⎥⎤=[e1′,e2′,e3′]⎣⎢⎡α1′α2′α3′⎦⎥⎤
为了将等式左边系数简化为单位阵,将上述等式左右两边左乘
[e1,e2,e3]T,即:
⎣⎢⎡α1α2α3⎦⎥⎤=⎣⎢⎡e1Te1′e21Te2′e3Te3′e1Te2′e2Te2′e3Te2′e1Te3′e1Te3′e3Te3′⎦⎥⎤⎣⎢⎡α1′α2′α3′⎦⎥⎤=Ra′
上式中中间的矩阵定义为旋转矩阵,我们可以从定义看出,旋转矩阵为一个行列式为1的正交矩阵,即逆为自身的矩阵。
在使用旋转矩阵表达三维世界中刚体的运动方式时,我们需要对其进行估计和优化。例如在优化位姿T时,就需要构建一个残差方程,也就是估计值与观测值之间的误差,而此时需要求解残差方程J对变化矩阵T的求导,变成了矩阵求导问题,而且对于变换矩阵是并不封闭的,所谓的不封闭就是两个变换矩阵相加得到的并不是变换矩阵,而不像是实数1加上实数2得到的3仍然是一个实数。
除此之外,由上面的变换矩阵的定义我们知道,旋转矩阵本身是带有约束的矩阵,也就是旋转矩阵为一个行列式为1的正交矩阵,额外的约束会增加优化的困难,为了简化求解的方式,引入了李群。
简单的说群就是一种集合加上一种运算的代数结构。虽然旋转矩阵对于加法是不封闭的,但是对于乘法是封闭的,两个旋转矩阵相乘代表做了两次旋转。 对于旋转矩阵和变换矩阵的群定义如下:
SO(3)={R∈R3×3∣RRT=I,det(R)=1}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),T∈R3}
所谓的李群是指具有连续(光滑)性质的群,S0(3)和SE(3)在实数空间上都是连续的,所以他们都是李群。到此为止李群的引入解决了变换矩阵额外约束的问题,
接下来解决矩阵的求导问题。首先对于任意矩阵R,假设R是某个刚体的旋转,它会随着时间连续变换即为时间的函数R(t),因此:
R(t)R(t)T=I
对时间t进行求导,其中
R(t)^代表函数R(t)的导数
R(t)^R(t)T+R(t)R(t)^T=0
通过移项可以得到:
R(t)^R(t)T=−R(t)R(t)^T=−(R(t)^R(t)T)T
显然,
R(t)^R(t)T是一个反对称函数。其特征是主对角线上的元素是0,关于主对角线对称的元素互为相反数。因此对于任意反对称矩阵,总能找到一个唯一与之对应的向量。
a∧=A=⎣⎢⎡0a3−a2−a30a1a2−a10⎦⎥⎤
因此对于反对称矩阵
R(t)^R(t)T,同样也可以找到一个三维向量
ϕ(t)∈R3,即:
R(t)^R(t)T=ϕ(t)∧
等式两边右乘
R(t),且R为正交矩阵,所以得到:
R(t)^=ϕ(t)∧R(t)=⎣⎢⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎥⎤R(t)
至此我们就得到了矩阵的导数,每次对旋转矩阵求导只需要左乘
ϕ(t)即可。李代数描述了旋转矩阵R局部的导数关系,也叫正切空间。
下面给出旋转矩阵和变换矩阵的群所对应的李代数:
so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
注:
ξ是一个六维向量(3+3),且在se(3)中符号∧表示将六维向量转成六维矩阵 ,并不是代表反对称。
因此,引入李群的目的是解决旋转矩阵本身的约束问题,简化后面的优化过程。引入李代数是为了解决旋转矩阵的求导问题,当然李群和李代数之间也存在指数映射和对数映射的相互转换。
关于Vins-Mono学习的过程中问题记录
1.关于参数 max_solver_time
最大求解时间 在VINS中默认为0.04,迭代次数最多为8次,在实际使用中该参数需要根据性能调试,确保迭代次数足够,否则很难有一个很好的优化结果!
2.在IMU参数中,噪声和随机游走会直接影响到IMU的协方差矩阵,关于IMU的噪声,在标定的情况下是静止的,但是在运动时使用,因此一般将此扩大10~15倍去使用。
3.关于IMU和Cam的时间戳同步,理想的情况下是实现时间戳的硬同步。
4.在IMU、图像、后端三个线程中,后端需要处理IMU和图像,找到合适的图像和图像之间的IMU数据。因此该三个线程共用一个 m_buf
线程互斥锁。在后端process()
中通过调用wait()
自动调用m_buf.lock()
来加锁,然后去寻找数据。如果未得到则线程被阻塞,自动调用m_buf.unlock()
释放锁,使得IMU和图像的线程可以继续向BUF中添加数据,每当添加数据后都会调用con.notify_one
唤醒process()
线程。
5.在readImage对图像处理中,关于直方图均衡化,可增强图像的对比度。
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
复制代码
在VINS中默认为3.0,如果将其增大,对比度会增加,同时噪声点也会增加。 6.在VINS中的光流跟踪也是直接调用opencv
中的函数,其中两个关键参数,一个为maxLevel
为金字塔层数,还有一个则为flags
,设置下一帧特征点的初始位置,如果为空则表示与上一帧相同。
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
复制代码
在VINS-MONO中设置为空。
7.协方差矩阵即不确定性,随着IMU帧数的增加,噪声的不确定性也增加。在IMU和视觉之间,通过对比协方差矩阵来判断更相信谁,其两者为一个相对的关系。
8.对于每一帧得到的图像,将当前帧看到的特征点image
放到特征点管理器f_manager
中,然后进行滑窗优化。image
表示当前帧跟踪到上一帧的特征点集合。放入特征点管理器中,将当前帧与地图中的其他帧建立数据关联,若为已知点,则加到共视关系中,若为新点,则新建立一个特征点。
9.在SLAM后端中,首先确定所有的约束关系,然后针对每个约束确定三个要素:误差项、优化变量、协方差。然后计算Jacobian,调用GN、LM等求解BA。
10.在更新滑动窗口的过程中,要不去掉最老帧,要不去掉次新帧。在去掉最老帧的时候,该帧所看到的路标点需要更新该路标点的第一帧,IMU预积分信息舍弃掉。而在去掉次新帧时,次新帧上一帧到下一帧的两段预积分合并。