[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}}$,两个旋转量满足：
\[R_b^c R_{b_k}^{b_{k+1}}=R_{c_k}^{c_{k+1}}R_b^c \tag{1}\]
四元数表示，则有
\[q_b^c \otimes q_{b_k}^{b_{k+1}}-q_{c_k}^{c_{k+1}} \otimes q_b^c = 0 \tag{2}\]
将四元数乘法运算化为一个\(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_a \otimes q_b=\left[\begin{array}{c}{ w_b x_a+ z_b y_a-y_b z_a + x_b w_a}\\{- z_b x_a+ w_b y_a+ x_b z_a+ y_b w_a}\\{ y_b x_a- x_b y_a + w_b z_a+ z_b w_a}\\{- x_b x_a- y_b y_a- z_b z_a+ w_b 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]\left[\begin{array}{c} x_a \\y_a\\z_a\\w_a \end{array} \right] \tag{3}\]
记\(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]\), 则有：
\[q_a \otimes q_b=\left[\begin{array}{c}w_bI-q_{x_b y_b z_b}^{\wedge}& q_{x_b y_b z_b}^{T}\\-q_{x_b y_b z_b} & w_b \end{array}\right]\left[\begin{array}{c} x_a \\y_a\\z_a\\w_a \end{array} \right] 记Q_R(q_{xyz})=\left[\begin{array}{c}w_I-q_{x y z}^{\wedge}& q_{x y z}\\-q_{x y z}^T & w \end{array}\right]_{4 \times 4} \tag{4}\]
同样的，我们整理\((3)\)式按照\(b\)排序，则有:
\[q_a \otimes q_b=\left[\begin{array}{c}{ w_a x_b - z_a y_b+ y_a z_b+ x_a w_b}\\{ z_a x_b+ w_a y_b- x_a z_b+ y_a w_b}\\{ - y_a x_b+ x_a y_b + w_a z_b + z_a w_b}\\{- x_a x_b- y_a y_b- z_a z_b+ w_a w_b} \end{array}\right]= \left[\begin{array}{c}w_a & -z_a & y_a & x_a\\ z_a & w_a & -x_a & y_a\\-y_a & x_a & w_a & z_a\\ -x_a & -y_a &-z_a & w_a\end{array} \right]\left[\begin{array}{c} x_b \\y_b\\z_b\\w_b \end{array} \right] \tag{5}\]
那么，同样有我们就有左乘法四元数的矩阵：
\[q_a \otimes q_b=\left[\begin{array}{c}w_aI+q_{x_ay_az_a}^{\wedge}& q_{x_ay_az_a}^{T}\\-q_{x_ay_az_a} & w_a \end{array}\right]\left[\begin{array}{c} x_b \\y_b\\z_b\\w_b \end{array} \right] 记Q_L(q_{xyz})=\left[\begin{array}{c}w_I+q_{x y z}^{\wedge}& q_{x y z}\\-q_{x y z}^T & w \end{array}\right]_{4 \times 4}\tag{6}\]
然后，我们整理\((2)\)式得到：
\[\left(Q_R(q_{b_k}^{b_{k+1}})-Q_L(q_{c_k}^{c_{k+1}})\right)q_b^c=0 记A=Q_R(q_{b_k}^{b_{k+1}})-Q_L(q_{c_k}^{c_{k+1}}) \tag{7}\]
取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]。
\[ A=U_{4n \times 4n} \Sigma_{4n \times 4} V^T_{4 \times 4} \]
最小二乘解即为最小奇异值对应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;
}参考：
[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 估计器之外参初始化
标签：begin,right,end,标定,IMU,array,VINS,left 来源： https://www.cnblogs.com/dzqiu/p/11427047.html