[VINS]IMU与相机之间旋转量的标定
VINS-Mono[1]中IMU-Camera外参旋转量\(R_b^c\)的计算方法在他们实验室发的之前的论文有详细讲解[2]。视觉利用匹配特征点中的基础矩阵求出相机坐标系下两帧的旋转量\(R_{c_k}^{c_{k+1}}\),通过IMU预积分得到的两帧之间IMU坐标系下的旋转量$ R_{b_k}^{b_{k+1}}$,两个旋转量满足:
\]
四元数表示,则有
\]
将四元数乘法运算化为一个\(4 \times 4\)的矩阵运算,YouTube上有个很好的视频讲解[3]。伯克利CS184也作出很好的讲解[4],使用行向量表示四元数,推过程类似。这里做简单的归纳讲述。两个四元数分别为:\(q_a=\left[\begin{array} {c}{x_a}\\{y_a}\\{z_a}\\{w_a} \end{array}\right]\),\(q_b=\left[\begin{array} {c}{x_b}\\{y_b}\\{z_b} \\{w_b}\end{array}\right]\)。更具四元数乘法规则,可以得到:
\]
记\(q_{xyz}=\left[\begin{array}{c}x & y & z \end{array} \right]\),\(q_{xyz}^{\wedge}=\left[\begin{array}{c}0 & -z & y \\ z & 0 & -x \\ -y & z & 0 \end{array} \right]\), 则有:
\]
同样的,我们整理\((3)\)式按照\(b\)排序,则有:
\]
那么,同样有我们就有左乘法四元数的矩阵:
\]
然后,我们整理\((2)\)式得到:
\]
取IMU旋转角\(\left\{q_{b_k} ^{b_{k+1}},q_{b_{k+1}} ^{b_{k+2}}...q_{b_{k+n-1}} ^{b_{k+n}}\right\}\),相机旋转角\(\left\{q_{ck} ^{c_{k+1}},q_{c_{k+1}} ^{c_{k+2}}...q_{c_{k+n-1}} ^{c_{k+n}}\right\}\),构建矩阵\(\rm{A}_{4n \times 4}\) 使用SVD分解该最小二乘问题[5]。
\]
最小二乘解即为最小奇异值对应V的特征向量,即\(A^TA\)最小特征值对应V的特征向量。在VINS-Mono中,加入每组旋转角相差的加入权重Huber,再去最后的一列的特征向量作为最小二乘的解。
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
Rc.push_back(solveRelativeR(corres));//帧间cam的R,由对极几何得到
Rimu.push_back(delta_q_imu.toRotationMatrix());//帧间IMU的R,由IMU预积分得到
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每帧IMU相对于起始帧IMU的R
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]);
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;
++sum_ok;
Matrix4d L, R;
//求Q_L
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;
//求Q_R
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);
}
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3); //取最后一列,最小奇异值对应的特征向量为最优解
Quaterniond estimated_R(x);
ric = estimated_R.toRotationMatrix().inverse();
//cout << svd.singularValues().transpose() << endl;
//cout << ric << endl;
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
}
[[3]](#CS184),另外一种方法是,设两个四元数分别为:$q_a=\left[\begin{array} {c}{x_a}\end{array}\right]$,$q_b=\left[\begin{array} {c}{x_b}\end{array}\right]$。
$$q_a \otimes q_b=\left[\begin{array}{c}{x_a w_b+y_a z_b-z_a y_b+w_a x_b}\\{-x_a z_b+y_a w_b+z_a x_b+w_a y_b}\\{x_a y_b-y_a x_b + z_a w_b+w_a z_b}\\{-x_a x_b-y_a y_b-z_a z_b+w_a w_b} \end{array}\right]^T= \left[\begin{array}{c} x_a &y_a&z_a&w_a \end{array} \right]\left[\begin{array}{c}w_b & -z_b & y_b & -x_b\\ z_b & w_b & -x_b & -y_b\\-y_b & x_b & w_b & -z_b\\ x_b & y_b &z_b & w_b\end{array} \right]$$ -->
参考:
[1]VINS-Mono
[2]Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration
[3]Quaternions as 4x4 Matrices - Connections to Linear Algebra
[4]CS184: Using Quaternions to Represent Rotation
[5]SVD分解及线性最小二乘问题
[6]VINS 估计器之外参初始化
最新文章
- CSS基本知识0-命名规范
- Android学习--摘录
- 虚拟化之vmware虚拟机扩容与克隆
- HL7及PIX相关的测试工具
- 如何在ubuntu下使用stage3d的硬件加速
- Cocos2d-x 使用物理引擎进行碰撞检测
- C++ 面向对象学习2 构造方法
- nefu 446 今年暑假不AC(贪心)
- Map value类型不同的写法
- postgresql :: FATAL: could not write init file
- 第20月第14天 objc_getAssociatedObject _cmd
- TFS Release 步骤调用命令行返回失败信息的处理方法
- 8-Python3从入门到实战—基础之数据类型(集合-Sets)
- 计数器控件实例 NumericStepper
- java操作linux 提交spark jar
- Eclipse代码提示补全问题,自动选择第一个
- 文本比较算法Ⅱ——Needleman/Wunsch算法的C++实现【求最长公共子串(不需要连续)】
- 002Conditional条件化创建bean
- WEB打印控件Lodop(V6.x)使用说明及样例
- Redis中关于Hash键的一些问题