IMU 坐标系 与 相机 坐标系旋转对齐
四元数的左乘和右乘
q
1
⊗
q
2
=
[
q
1
]
L
q
2
,
q
1
⊗
q
2
=
[
q
2
]
R
q
1
q_1 \otimes q_2 = \left[q_1\right]_L q_2, \qquad q_1 \otimes q_2 = \left[q_2\right]_R q_1
q1⊗q2=[q1]Lq2,q1⊗q2=[q2]Rq1
[
q
]
L
=
q
w
I
+
[
0
−
q
v
T
q
v
[
q
v
]
×
]
,
[
q
]
R
=
q
w
I
+
[
0
−
q
v
T
q
v
−
[
q
v
]
×
]
\left[q\right]_L=q_wI + \begin{bmatrix} 0 & -{q_v}^T \\ q_v & \left[q_v\right]_{\times} \end{bmatrix}, \qquad \left[q\right]_R=q_wI + \begin{bmatrix} 0 & -{q_v}^T \\ q_v & -\left[q_v\right]_{\times} \end{bmatrix}
[q]L=qwI+[0qv−qvT[qv]×],[q]R=qwI+[0qv−qvT−[qv]×]
其中:
[
a
]
×
≜
[
0
−
a
z
a
y
a
z
0
−
a
x
−
a
y
a
x
0
]
\left[a\right]_{\times}\triangleq \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix}
[a]×≜⎣⎡0az−ay−az0axay−ax0⎦⎤
[
q
]
L
=
[
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
]
,
[
q
]
R
=
[
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
]
\left[q\right]_L= \begin{bmatrix} 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{bmatrix}, \qquad \left[q\right]_R= \begin{bmatrix} 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{bmatrix}
[q]L=⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤,[q]R=⎣⎢⎢⎡qwqxqyqz−qxqw−qzqy−qyqzqw−qx−qz−qyqxqw⎦⎥⎥⎤
相机 与 IMU之间的相对旋转对齐
假设相机与IMU之间存在一个相对旋转 Q i c Q_{ic} Qic, Q i c Q_{ic} Qic表示从IMU转到相机的四元数。
根据以上条件可得到方程:
Q i c ⊗ Q i m u ⊗ Q i c − 1 = Q c a m Q_{ic} \otimes Q_{imu} \otimes {Q_{ic}^{-1}} = Q_{cam} Qic⊗Qimu⊗Qic−1=Qcam
Q i c ⊗ Q i m u = Q c a m ⊗ Q i c Q_{ic} \otimes Q_{imu} = Q_{cam} \otimes Q_{ic} Qic⊗Qimu=Qcam⊗Qic
根据四元数的左乘和右乘得到:
[ Q i m u ] R Q i c = [ Q c a m ] L Q i c \left[Q_{imu}\right]_RQ_{ic} = \left[Q_{cam}\right]_LQ_{ic} [Qimu]RQic=[Qcam]LQic
⇒ ( [ Q c a m ] L − [ Q i m u ] R ) Q i c = 0 \Rightarrow \left(\left[Q_{cam}\right]_L - \left[Q_{imu}\right]_R\right)Q_{ic} = 0 ⇒([Qcam]L−[Qimu]R)Qic=0
具体代码:
代码中会通过迭代的方式计算
Q
i
c
Q_{ic}
Qic,同时计算的过程中会加一些权重。
bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
//! Step1:滑窗内的帧数加1
frame_count++;
//! Step2:计算两帧之间的旋转矩阵
Rc.push_back(solveRelativeR(corres));
Rimu.push_back(delta_q_imu.toRotationMatrix());
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);
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]);
//!Step3:求取估计出的相机与IMU之间旋转的残差
//!r = acos((tr(R_cbRbR_bcR_c)-1)/2)
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
//! Step4:计算外点剔除的权重
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
++sum_ok;
Matrix4d L, R;
//! Step5:求取系数矩阵
//! 得到相机对极关系得到的旋转q的左乘
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;
//! 得到由IMU预积分得到的旋转q的右乘
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);
}
//!Step6:通过SVD分解,求取相机与IMU的相对旋转
//!解为系数矩阵A的右奇异向量V中最小奇异值对应的特征向量
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_R(x);
ric = estimated_R.toRotationMatrix().inverse();
//!Step7:判断对于相机与IMU的相对旋转是否满足终止条件
//!1.用来求解相对旋转的IMU-Cmaera的测量对数是否大于滑窗大小。
//!2.A矩阵第二小的奇异值是否大于某个阈值,使A得零空间的秩为1
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;
}
}