在VINS中相机的外参 R i c R_{ic} Ric?是可以在线动态标定的，实现函数为：
/**
* @brief 外参在线标定
*
* @param corres 两帧图像之间的共视特征点
* @param delta_q_imu 两帧图像之间的IMU预积分量
* @param calib_ric_result 标定结果存放
* @return true
* @return false
*/
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
基本原理
R c b R b k b k + 1 R c b T = R c k c k + 1 ? R c b R b k b k + 1 = R c k c k + 1 R c b ? q c b ? q b k b k + 1 = q c k c k + 1 ? q c b ? [ q b k b k + 1 ] R q c b = [ q c k c k + 1 ] L q c b ? { [ q c k c k + 1 ] L ? [ q b k b k + 1 ] R } q c b = 0 \begin{aligned} &R_{c b} R_{b_{k} b_{k+1}} R_{c b}^{T}=R_{c_{k} c_{k+1}} \\ &\Rightarrow R_{c b} R_{b_{k} b_{k+1}}=R_{c_{k} c_{k+1}} R_{c b} \\ &\Rightarrow q_{c b} \otimes q_{b_{k} b_{k+1}}=q_{c_{k} c_{k+1}} \otimes q_{c b} \\ &\Rightarrow\left[q_{b_{k} b_{k+1}}\right]_{R} q_{c b}=\left[q_{c_{k} c_{k+1}}\right]_{L} q_{c b} \\ &\Rightarrow\left\{\left[q_{c_{k} c_{k+1}}\right]_{L}-\left[q_{b_{k} b_{k+1}}\right]_{R}\right\} q_{c b}=0 \end{aligned} ?Rcb?Rbk?bk+1??RcbT?=Rck?ck+1???Rcb?Rbk?bk+1??=Rck?ck+1??Rcb??qcb??qbk?bk+1??=qck?ck+1???qcb??[qbk?bk+1??]R?qcb?=[qck?ck+1??]L?qcb??{[qck?ck+1??]L??[qbk?bk+1??]R?}qcb?=0?
其中涉及到四元数的左乘和右乘矩阵：
q = [ x y z w ] T = [ q v q w ] , [ q ] L = q w I + [ [ q v ] × q v ? q v T 0 ] , [ q ] R = q w I + [ ? [ q v ] × q v ? q v T 0 ] q=[x y z w]^{T}=\left[\begin{array}{c} q_{v} \\ q_{w} \end{array}\right], \quad[q]_{L}=q_{w} I+\left[\begin{array}{cc} {\left[q_{v}\right]_{\times}} & q_{v} \\ -q_{v}^{T} & 0 \end{array}\right], \quad[q]_{R}=q_{w} I+\left[\begin{array}{cc} -\left[q_{v}\right]_{\times} & q_{v} \\ -q_{v}^{T} & 0 \end{array}\right] q=[xyzw]T=[qv?qw??],[q]L?=qw?I+[[qv?]×??qvT??qv?0?],[q]R?=qw?I+[?[qv?]×??qvT??qv?0?]
当有多对这样的组合后就可以构建超定方程即公式
A
=
{
[
q
c
k
c
k
+
1
1
]
L
?
[
q
b
k
b
k
+
1
1
]
R
[
q
c
k
c
k
+
1
2
]
L
?
[
q
b
k
b
k
+
1
2
]
R
?
[
q
c
k
c
k
+
1
n
]
L
?
[
q
b
k
b
k
+
1
n
]
R
}
,
A
?
q
c
b
=
0
A=\left\{\begin{array}{c} {\left[q_{c_{k} c_{k+1}}^{1}\right]_{L}-\left[q_{b_{k} b_{k+1}}^{1}\right]_{R}} \\ {\left[q_{c_{k} c_{k+1}}^{2}\right]_{L}-\left[q_{b_{k} b_{k+1}}^{2}\right]_{R}} \\ \cdots \\ {\left[q_{c_{k} c_{k+1}}^{n}\right]_{L}-\left[q_{b_{k} b_{k+1}}^{n}\right]_{R}} \end{array}\right\}, \quad A \cdot q_{c b}=0
A=??????????????[qck?ck+1?1?]L??[qbk?bk+1?1?]R?[qck?ck+1?2?]L??[qbk?bk+1?2?]R??[qck?ck+1?n?]L??[qbk?bk+1?n?]R????????????????,A?qcb?=0
通过SVD分解就可以得到 q c b q_{cb} qcb?，然后再取逆即可得到外参 q b c = i n v ( q c b ) q_{b c}=i n v\left(q_{c b}\right) qbc?=inv(qcb?)
代码实现
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
this->frame_count++; // 记录进入该函数的次数，用于定义A矩阵的规模的
Rc.push_back(solveRelativeR(corres));// 求取帧间变换阵R，（第k+1帧变换到第k帧的）R_c(k+1)^ck
Rimu.push_back(delta_q_imu.toRotationMatrix()); // 两帧之间的陀螺仪预积分值中的旋转变化量 R
Rc_g.push_back(ric.inverse() * delta_q_imu * ric); // 图像k帧到k+1帧的旋转
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;
// 构建四元数左乘矩阵
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);
// 这里得到的是R_ci，取逆可得 R_ic
ric = estimated_R.toRotationMatrix().inverse();
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
// 用0.25来限定倒数第二小的奇异值，如果全部奇异值都非常小接近0,则说明相机没有进行充分的旋转，无法进行初始化
// 至少会迭代windowSize次
if (frame_count >= windowSize && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
}
