在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}
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
=
[
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=[qvqw],[q]L=qwI+[[qv]×−qvTqv0],[q]R=qwI+[−[qv]×−qvTqv0]
当有多对这样的组合后就可以构建超定方程即公式
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=⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧[qckck+11]L−[qbkbk+11]R[qckck+12]L−[qbkbk+12]R⋯[qckck+1n]L−[qbkbk+1n]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;
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
