VINS 中的旋转外参初始化

​ 为了使这个两个传感器融合,我们首先需要做的事情是将两个传感器的数据对齐,除了时间上的对齐,还有空间上的对齐。空间上的对齐通俗的讲就是将一个传感器获取的数据统一到另一个传感器的坐标系中,其关键在于确定这两个传感器之前的外参,本文将详细介绍 VINS_Monocamera-imu 的旋转外参标定算法原理并对其代码进行解读(VINS-Fusion中也是一样的)。

​ 相机与 IMU 之间的相对旋转如下图所示:

​ 上图表示相机和 IMU 集成的系统从到的运动,其中视觉可以通过特征匹配求得到时刻的旋转增量,同时 IMU 也可以通过积分得到到时刻的旋转增量。两个求得的旋转增量应当是相同的,因此分别将在自己坐标系下求得的增量转移到对方的坐标系下,即经过 \(\mathbf{q}_{bc}\) 变换后应当还是相同的。

​ 相邻两时刻 \(k\) , \(k + 1\) 之间有: IMU 旋转积分 \(\mathbf{q}_{b_{k} b_{k+1}}\) ,视觉测量 \(\mathbf{q}_{c_{k} c_{k+1}}\)。则有:

\[\mathbf{q}_{b_{k} b_{k+1}} \otimes \mathbf{q}_{b c}=\mathbf{q}_{b c} \otimes \mathbf{q}_{c_{k} c_{k+1}}
\]

​ 上面的式子可以写成:

\[\left(\left[\mathbf{q}_{b_{k} b_{k+1}}\right]_{L}-\left[\mathbf{q}_{c_{k} c_{k+1}}\right]_{R}\right) \mathbf{q}_{b c}=\mathbf{Q}_{k+1}^{k} \cdot \mathbf{q}_{b c}=\mathbf{0}
\]

​ 其中 \(\left[ \dot{\ \ \ } \right]_L, \left[ \dot{\ \ \ } \right]_R\) 表示 left and right quaternion multiplication,不清楚可以看附录,其实就是四元数乘法换了个表达形式。

​ 将多个时刻线性方程累计起来,并加上鲁棒核权重得到:

\[\begin{array}{c}
{\left[\begin{array}{c}
w_{1}^{0} \cdot \mathbf{Q}_{1}^{0} \\
w_{2}^{1} \cdot \mathbf{Q}_{2}^{1} \\
\vdots \\
w_{N}^{N-1} \cdot \mathbf{Q}_{N}^{N-1}
\end{array}\right] \mathbf{q}_{b c}=\mathbf{Q}_{N} \cdot \mathbf{q}_{b c}=\mathbf{0}} \\
w_{k+1}^{k}=\left\{\begin{array}{ll}
1, & r_{k+1}^{k}<\text { threshold } \\
\frac{\text { threshold }}{r_{k+1}^{k}}, & \text { otherwise }
\end{array}\right.
\end{array}
\]

该线性方程为超定方程,有最小二乘解。采用 SVD 进行求解。

  • 代码
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
Rc.push_back(solveRelativeR(corres)); // 对极几何计算出的 R,t 约束
Rimu.push_back(delta_q_imu.toRotationMatrix()); // IMU 预积分的得到的 R,t 约束
Rc_g.push_back(ric.inverse() * delta_q_imu * ric); // 将imu预积分转换到相机坐标系 Eigen::MatrixXd A(frame_count * 4, 4);
A.setZero();
int sum_ok = 0;
for (int i = 1; i <= frame_count; i++)
{
Quaterniond r1(Rc[i]);
Quaterniond r2(Rc_g[i]); // A.angularDistance(B):是求两个旋转之间的角度差,用弧度表示
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
ROS_DEBUG(
"%d %f", i, angular_distance); double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0; // huber 核函数作加权
++sum_ok;
// L R 分别为左乘和右乘矩阵
Matrix4d L, R; double w = Quaterniond(Rc[i]).w();
Vector3d q = Quaterniond(Rc[i]).vec();
L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
L.block<3, 1>(0, 3) = q;
L.block<1, 3>(3, 0) = -q.transpose();
L(3, 3) = w; Quaterniond R_ij(Rimu[i]);
w = R_ij.w();
q = R_ij.vec();
R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
R.block<3, 1>(0, 3) = q;
R.block<1, 3>(3, 0) = -q.transpose();
R(3, 3) = w; A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
} // svd 分解中最小奇异值对应的右奇异向量作为旋转四元数
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_R(x);
// 所求的 x 是q^b_c,在最后需要转换成旋转矩阵并求逆。
ric = estimated_R.toRotationMatrix().inverse();
//cout << svd.singularValues().transpose() << endl;
//cout << ric << endl;
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
// 至少迭代计算了WINDOW_SIZE次,且R的奇异值大于0.25才认为标定成功
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
}

Appendix

  • The product of two quaternions is bi-linear and can be expressed as two equivalent matrix products, namely
\[\mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{1}\right]_{L} \mathbf{q}_{2} \quad \text { and } \quad \mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{2}\right]_{R} \mathbf{q}_{1},
\]

\(\quad\)where \([\mathbf{q}]_{L}\) and \([\mathbf{q}]_{R}\) are respectively the left- and right- quaternion-product matrices

\[[\mathbf{q}]_{L}=\left[\begin{array}{cccc}
q_{w} & -q_{x} & -q_{y} & -q_{z} \\
q_{x} & q_{w} & -q_{z} & q_{y} \\
q_{y} & q_{z} & q_{w} & -q_{x} \\
q_{z} & -q_{y} & q_{x} & q_{w}
\end{array}\right], \quad[\mathbf{q}]_{R}=\left[\begin{array}{cccc}
q_{w} & -q_{x} & -q_{y} & -q_{z} \\
q_{x} & q_{w} & q_{z} & -q_{y} \\
q_{y} & -q_{z} & q_{w} & q_{x} \\
q_{z} & q_{y} & -q_{x} & q_{w}
\end{array}\right],
\]

\(\quad\)then

\[[\mathbf{q}]_{L}=q_{w} \mathbf{I}+\left[\begin{array}{cc}
0 & -\mathbf{q}_{v}^{\top} \\
\mathbf{q}_{v} & {\left[\mathbf{q}_{v}\right]_{\times}}
\end{array}\right], \quad[\mathbf{q}]_{R}=q_{w} \mathbf{I}+\left[\begin{array}{cc}
0 & -\mathbf{q}_{v}^{\top} \\
\mathbf{q}_{v} & -\left[\mathbf{q}_{v}\right]_{\times}
\end{array}\right]
\]

VINS中旋转外参初始化的更多相关文章

  1. VINS(四)初始化与相机IMU外参标定

    和单目纯视觉的初始化只需要获取R,t和feature的深度不同,VIO的初始化话通常需要标定出所有的关键参数,包括速度,重力方向,feature深度,以及相机IMU外参$R_{c}^{b}$和$p_{ ...

  2. VI ORB-SLAM初始化与VINS初始化对比(将vi orb-slam初始化方法移植到vins中)

    初始化时需要求出的变量:相机和imu外参r t.重力g.尺度s.陀螺仪和加速度计偏置ba bg. 下面对两种算法初始化的详细步骤进行对比: 求陀螺仪偏置bg 求解公式相同,求解方法不同.公式如下,VI ...

  3. VINS 估计器之外参初始化

    为何初始化外参 当外参完全不知道的时候,VINS也可以在线对其进行估计(rotation),先在processImage内进行初步估计,然后在后续优化时,会在optimize函数中再次优化. 如何初始 ...

  4. 相机-imu外参校准总结

    1. 研究背景及相关工作 1)研究背景 单目视觉惯性slam是一种旨在跟踪移动平台的增量运动并使用来自单个车载摄像头和imu传感器的测量结果同时构建周围环境地图的技术.视觉相机和惯性测量单元(imu) ...

  5. 相机imu外参标定

    1. 第一步初始化imu外参(可以从参数文档中读取,也可以计算出),VINS中处理如下: # Extrinsic parameter between IMU and Camera. estimate_ ...

  6. 解放双手——相机与IMU外参的在线标定

    本文作者 沈玥伶,公众号:计算机视觉life,编辑部成员 一.相机与IMU的融合 在SLAM的众多传感器解决方案中,相机与IMU的融合被认为具有很大的潜力实现低成本且高精度的定位与建图.这是因为这两个 ...

  7. C++类中成员变量的初始化总结(转帖)

    本文转自:C++类中成员变量的初始化总结 1. 普通的变量:      一般不考虑啥效率的情况下 可以在构造函数中进行赋值.考虑一下效率的可以再构造函数的初始化列表中进行.  1 class CA  ...

  8. 关于map容器的元素被无参初始化

    使用C++中的map容器定义一个mp,当你执行if语句判断mp[3]是否为1时,那么如果mp[3]以前不存在,此时mp[3]就会被无参初始化,second赋值为0. 以下的程序可以证明这一点.执行了第 ...

  9. C++类中成员变量的初始化总结

    @import url(http://i.cnblogs.com/Load.ashx?type=style&file=SyntaxHighlighter.css);@import url(/c ...

  10. 虚机中访问外网;NAT中的POSTROUTING是怎么搞的?

    看下docker中是怎么配置的网络 在虚机中访问外网:设定了qemu,在主机上添加路由:sudo iptables -t nat -I POSTROUTING -s 192.168.1.110 -j ...

随机推荐

  1. 基于Gazebo搭建移动机器人,并结合SLAM系统完成定位和建图仿真

    博客地址:https://www.cnblogs.com/zylyehuo/ gazebo小车模型创建及仿真详见之前博客 gazebo小车模型(附带仿真环境) - zylyehuo - 博客园 gaz ...

  2. 函数strncpy和memcpy的区别

    1定义 1.1 memcpy void *memcpy(void *destin, void *source, unsigned n); 参数 *destin ---- 需要粘贴的新数据(地址) *s ...

  3. Python基础 - 逻辑运算符

    Python语言支持逻辑运算符,以下假设变量 a 为 10, b为 20: 运算符 逻辑表达式 描述 实例 and x and y 布尔"与" - 如果 x 为 False,x a ...

  4. P3498 [POI2010]KOR-Beads 题解

    前言: 最近在做哈希的题,发现了这道好题,看题解里很多大佬的方法都很巧妙,自己就发一个较为朴素的方法吧. 题意: 题目传送门 给你一个序列,需要求出数 k,使划分的子串长度为 k 时,不同的子串数量最 ...

  5. Python与TensorFlow:如何高效地构建和训练机器学习模型

    目录 标题:<Python 与 TensorFlow:如何高效地构建和训练机器学习模型> 一.引言 随着人工智能的快速发展,机器学习作为其中的一个重要分支,受到了越来越多的关注和应用.而P ...

  6. 机器翻译技术的发展趋势:从API到深度学习

    目录 机器翻译技术的发展趋势:从API到深度学习 随着全球化的发展,机器翻译技术在各个领域得到了广泛的应用.机器翻译技术的核心是将源语言文本翻译成目标语言文本,其中涉及到语言模型.文本生成模型和翻译模 ...

  7. FFmpeg合并视频和音频文件

    使用IDM下载Bilibili的视频会出现音视频分离的问题,通常文件大的是视频(没有声音),文件小的是单独的音频. 将两个文件都下载下来后,可以使用FFmpeg将其合并成一个视频文件.首先去FFmpe ...

  8. 基于百度AI平台的人脸识别评分小程序

    face-recognition-scoring-applet 开放源代码,遵循Apache License 2.0 效果展示 可切换摄像头.拍照.从相册选择 效果预览 小程序账号注册及配置 地址:h ...

  9. CF1654E Arithmetic Operations 题解

    摘自我的洛谷博客. 题目让我们求改变数字的最少次数,那我们转化一下, 求可以保留最多的数字个数 \(cnt\),再用 \(n\) 减一下就行,即 \(res = n - cnt\). 我们先考虑两种暴 ...

  10. Java面试题全集(一)

    JDK.JRE.JVM之间的区别 JDK(Java SE Development Kit),Java标准开发包,它提供了编译.运⾏Java程序所需的各种⼯具和资源,包括Java编译器.Java运⾏时环 ...