VINS 外参在线标定

在VINS中相机的外参RicR_{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)

基本原理

RcbRbkbk+1RcbT=Rckck+1⇒RcbRbkbk+1=Rckck+1Rcb⇒qcb⊗qbkbk+1=qckck+1⊗qcb⇒[qbkbk+1]Rqcb=[qckck+1]Lqcb⇒{[qckck+1]L−[qbkbk+1]R}qcb=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} RcbRbkbk+1RcbT=Rckck+1⇒RcbRbkbk+1=Rckck+1Rcb⇒qcb⊗qbkbk+1=qckck+1⊗qcb⇒[qbkbk+1]Rqcb=[qckck+1]Lqcb⇒{[qckck+1]L−[qbkbk+1]R}qcb=0

其中涉及到四元数的左乘和右乘矩阵：
q=[xyzw]T=[qvqw],[q]L=qwI+[[qv]×qv−qvT0],[q]R=qwI+[−[qv]×qv−qvT0]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=[qvqw],[q]L=qwI+[[qv]×−qvTqv0],[q]R=qwI+[−[qv]×−qvTqv0]

当有多对这样的组合后就可以构建超定方程即公式
A={[qckck+11]L−[qbkbk+11]R[qckck+12]L−[qbkbk+12]R⋯[qckck+1n]L−[qbkbk+1n]R},A⋅qcb=0A=\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=⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧[qckck+11]L−[qbkbk+11]R[qckck+12]L−[qbkbk+12]R⋯[qckck+1n]L−[qbkbk+1n]R⎭⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎫,A⋅qcb=0

通过SVD分解就可以得到 qcbq_{cb}qcb，然后再取逆即可得到外参 qbc=inv(qcb)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)^ckRimu.push_back(delta_q_imu.toRotationMatrix()); // 两帧之间的陀螺仪预积分值中的旋转变化量 RRc_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_icric = 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;}elsereturn false;
}