（二）VINS系统---视觉惯性对齐（Visual-Inertial Alignment）
发布时间
阅读量：
阅读量
VINS-Fusion
1. 陀螺仪bias标定（线性初始化，只在开始进行一次）
对应论文： 秦通大佬的论文
解释说明：初始化的时候，我们通过ALLAN方差标定有且只能获得IMU加速度和陀螺仪 bias的导数（一种随机变量）服从的高斯分布的方差 和 加性高斯白噪声 的方差。没有bias的数值。这里的方差只会用在预积分残差协方差更新的过程中使用。
我们实际的IMU值需要减去bias的值，bias的值肯定不能凭空得来；其实对于陀螺仪来说，有六面标定法等等方法可以获得bias（确定性误差），但是这需要高精度转台，实际大多数人不可能进行这种标定，因此我们就只能利用视觉信息和IMU的信息来进行标定了。这相对于提出了一种新的标定方法。只需要视觉传感器和IMU。但是初始的时候肯定不准，因为最开始我们假定了bias=0。
上面公式代表的含义如下：
该系统通过视觉传感器获取了IMU姿态预积分的结果。在推导这个公式的过程中你将发现, 这里未使用IMU的数据信息, 而仅依赖于SFM算法提取出各帧图像的姿态信息。
：补偿bias 后的 IMU 姿态预积分 。仅仅使用IMU信息**。**
内部包含了具有偏差的IMU的姿态预积分信息尚未对其进行补偿校正 。
理论上而言，在去除所有偏差后其结果即为真实状态下的姿态信息 。通过将此真实姿态信息与视觉传感器估计出的姿态信息进行四元数空间中的减法运算 ，必然得到的结果应为单位四元数。因此有
（8）
其中（7）式之所以写成这样，我们可以从下面的分析进行理解：
绕单位向量
旋转
角度，这种旋转用四元数表示为
当
，那么四元数微量就变成
理所应当地，（7）式中在利用误差形式的泰勒展开之后，便有一个
雅可比矩阵的由来参见（一）VINS系统框架--残差剖析_AutoGalaxy的博客-博客。
将公式（7）代入方程（8），从而得出结论：任何四元数乘以单位四元数都会得到其自身的结果。
定义
，即
只是虚部部分。
就有
，这只是一个图像帧得到的式子，事实上，应该有以下的式子成立
,
，... ，然后对应部分加起来即可得到一个方程组
求解这个方程组，就可以得到
。
上述代码对应下面这个函数，具体的步骤在代码里面了。
> 1. void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
>
> 2. {
>
> 3. Matrix3d A;
>
> 4. Vector3d b;
>
> 5. Vector3d delta_bg;
>
> 6. A.setZero();
>
> 7. b.setZero();
>
> 8. map<double, ImageFrame>::iterator frame_i;
>
> 9. map<double, ImageFrame>::iterator frame_j;
>
> 10. for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
>
> 11. {
>
> 12. frame_j = next(frame_i);
>
> 13. MatrixXd tmp_A(3, 3);
>
> 14. tmp_A.setZero();
>
> 15. VectorXd tmp_b(3);
>
> 16. tmp_b.setZero();
>
> 17. // 得到 q bk bk+1 = (q w bk)^(-1) * q (w bk+1)
>
> 18. Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
>
> 19. // 取出对应的 雅可比矩阵J
>
> 20. tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
>
> 21. // 取出 四元数的虚部 r bk+1 bk * q bk bk+1
>
> 22. tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
>
> 23. // 用 J^{T}J x = b 的方法求解，叠加所帧的
>
> 24. A += tmp_A.transpose() * tmp_A;
>
> 25. b += tmp_A.transpose() * tmp_b;
>
> 26.
>
> 27. }
>
> 28. delta_bg = A.ldlt().solve(b);
>
> 29. // ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
>
> 30.
>
> 31. for (int i = 0; i <= WINDOW_SIZE; i++)
>
> 32. Bgs[i] += delta_bg;
>
> 33.
>
> 34. for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
>
> 35. {
>
> 36. frame_j = next(frame_i);
>
> 37. frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
>
> 38. }
>
> 39. }
>
>
>
>
> ```
>
>
>
> 
>
>
>
> **这一部分取出的雅可比矩阵就是红色框框的地方。**
>
>
>
> **在求出bias后，需要重新计算一遍预积分。**
>
>
* * *
## 2\. 速度，重力向量，系下的尺度因子s 初始化
>
>
> 对应论文：
>
>
>
> 自己下载查看吧！
>
>
>
>
>
>
> 理论解析：
>
>
>
> 
>
>
>
> **因为我们的基础坐标系是C0系，而不是W系，因此需要进行重力向量的对齐，也就是求gc0；尺度对齐也是通过IMU的平移进行的；上一小节仅仅利用了旋转约束，进而得到了bias；这一节就利用平移的两个约束；最后顺便也优化一下速度。**
>
>
>
> ****
>
>
>
> * * *
>
>
>
> **优化，优化，最终目的还是离不开构建残差**
>
>
>
> * * *
>
>
>
> **可利用的约束只有位置预积分和速度预积分**
>
>
>
> * * *
>
>
>
> **在陀螺仪bias标定一小节，得到了预积分量新的（相对来说更加准确的）测量值**
>
>
>
> * * *
>
>
>
> **因此这一节只需要计算预积分量的估计值即可**
>
>
>
> * * *
>
>
>
> **而计算预积分的估计值一般是通过PVQ公式进行的
> （把w换成c0即可）**
>
>
>
> * * *
>
>
>
> 
>
>
>
>
>
> * * *
>
>
>
> **上面的蓝色框框就是我们的待优化变量，
> 总共是3+3+3+1 = 10维，然而我们的方程只有6个，
> 这样的话只能找一个最小二乘解**
>
>
>
> * * *
>
>
>
> **当然，为了结合上一小节的新的预积分量，还要构建这样的残差来求上面的最小二乘解**
>
>
>
> * * *
>
>
>
> 
>
>
>
> * * *
>
>
>
> (1)式代入(2)式可以得到：
>
>
>
>  
>
>
>
>
>
> **上面橙色的部分是要经过一定的变换求出来的，我们的SfM只能得出**
>
>
>
> 利用下面这个关系
>
>
>
> 
>
>
>
> 上式代入
>
>
>
> 
>
>
>
> 
>
>
>
> **上面只是一帧图像的残差，还有很多帧图像的残差，虽然速度 不一样，但是和始终是一样的。**
>
>
* * *
## 3\. 重力方向修正 （代码还悄悄更新了一下尺度s）
其实一开始看到这里的时候，我就有一些懵逼了，因为上一节不是已经求出系下的重力加速度了吗？为什么还要进行修正，这里其实我也不太懂原因，但是还是得弄懂这一节的修正方法是怎么实施的。最关键的一点就是一组三维向量，可由3个三维正交基线性表示。
>
>
> 论文自己去看原文，这里只给分析。
>
>
>
> 
>
>
>
> 其中的是事先定义的重力模长，已经固定了。因此重力加速度的自由度就只有两个了。
>
>
>
> 
>
>
>
> 
>
>
>
> 
>
>
>
> 
>
>
>
> 
>
>
>
> 这两个红框便是**新的** 专门用于修正重力向量的线性方程组，这只是一帧图像得到的方程组，很多帧图像构成一个超定方程组。
>
>
>
> **虽然说仅仅用于修正重力向量，但是实际上代码里面还悄悄地修正了一下尺度。**
>
>
>
>
>
>全部评论 (0)
还没有任何评论哟~