VINS-Mono融合轮速计和GPS(二):鲁棒初始化
2021/8/24 23:09:05
本文主要是介绍VINS-Mono融合轮速计和GPS(二):鲁棒初始化,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!
VINS-Mono融合轮速计和GPS(二):鲁棒初始化
- 开篇
- 介绍
- 理论
- 鲁棒初始化
- VINS-Mono的初始化
- 1. 陀螺仪bias校正
- 2. 初始化速度和重力
- 3. 初始化尺度因子
- 4. 优化重力方向
- 实践
- 配合代码查看
开篇
项目地址VINS-GPS-Wheel,欢迎交流学习。
VINS-Mono融合轮速计和GPS(一):预积分
VINS-Mono融合轮速计和GPS(二):鲁棒初始化
VINS-Mono融合轮速计和GPS(三):优化和在线标定
VINS-Mono融合轮速计和GPS(四):融合GPS
介绍
上一篇博客介绍了带有轮速计的预积分相关内容(工作有变动,托挺久的,实在抱歉),本篇用预积分的结果对初始化过程进行优化。
理论
鲁棒初始化
VINS-Mono的初始化
先说一下VINS-Mono的初始化过程。首先,用SFM求解滑窗内所有帧的位姿,和所有路标点的3D位置,然后跟IMU预积分的值对齐,求解重力方向、尺度因子、陀螺仪bias及每一帧对应的速度。
流程如下:
考虑到IMU初始化较慢,估计视觉尺度效果也容易受影响,所以改进思路是采用轮速计的观测数据进行视觉尺度的估计和参与初始速度的估计。具体改动为,图中前三个步骤采用原框架的视觉初始化模块,visualInitialAlign是对visualIMUAlignment之后的结果进行对齐。所以改动主要在visualIMUAlignment模块。
将visualIMUAlignment详细展开来说,又分为4个步骤:
1. 陀螺仪bias校正
(和VINS-Mono相同,暂略,有空再加)
2. 初始化速度和重力
p
b
k
b
0
=
p
o
k
b
0
−
R
b
k
b
0
p
o
b
p_{b_k}^{b_0}=p_{o_k}^{b_0}-R_{b_k}^{b_0}p_o^b
pbkb0=pokb0−Rbkb0pob
α
b
j
b
i
=
q
w
b
i
(
p
b
j
w
−
p
b
i
w
−
v
i
w
Δ
t
+
1
2
g
w
Δ
t
2
)
\alpha_{b_j}^{b_i} = q_{w}^{b_i}(p_{b_j}^w-p_{b_i}^w-v_i^w\Delta t+\frac{1}{2}g^w\Delta t^2)
αbjbi=qwbi(pbjw−pbiw−viwΔt+21gwΔt2)
β
b
j
b
i
=
q
w
b
i
(
v
j
w
−
v
i
w
+
g
w
Δ
t
)
\beta_{b_j}^{b_i} = q_{w}^{b_i}(v_j^w-v_i^w+g^w\Delta t)
βbjbi=qwbi(vjw−viw+gwΔt)
η
b
j
b
i
=
q
w
b
i
(
p
o
j
w
−
p
o
i
w
)
\eta_{b_j}^{b_i}=q_w^{b_i}(p_{o_j}^w-p_{o_i}^w)
ηbjbi=qwbi(pojw−poiw)
α
b
k
+
1
b
k
=
R
b
0
b
k
(
p
o
k
+
1
b
0
−
R
b
k
+
1
b
0
p
o
b
−
p
o
k
b
0
+
R
b
k
b
0
p
o
b
−
v
k
b
0
Δ
t
k
+
1
2
g
b
0
Δ
t
k
2
)
=
η
b
k
+
1
b
k
−
R
b
k
+
1
b
k
p
o
b
+
p
o
b
−
v
k
b
k
Δ
t
k
+
1
2
R
b
k
b
0
g
b
0
Δ
t
k
2
)
\begin{aligned} \alpha_{b_{k+1}}^{b_k}&=R_{b_0}^{b_k}(p_{o_{k+1}}^{b_0}-R_{b_{k+1}}^{b_0}p_{o}^b-p_{o_{k}}^{b_0}+R_{b_k}^{b_0}p_{o}^b-v_k^{b_0}\Delta t_k+\frac{1}{2}g^{b_0}\Delta t_k^2)\\&=\eta_{b_{k+1}}^{b_k}-R_{b_{k+1}}^{b_k}p_o^b+p_{o}^b-v_k^{b_k}\Delta t_k+\frac{1}{2}R_{b_k}^{b_0}g^{b_0}\Delta t_k^2) \end{aligned}
αbk+1bk=Rb0bk(pok+1b0−Rbk+1b0pob−pokb0+Rbkb0pob−vkb0Δtk+21gb0Δtk2)=ηbk+1bk−Rbk+1bkpob+pob−vkbkΔtk+21Rbkb0gb0Δtk2)
[
α
b
k
+
1
b
k
−
η
b
k
+
1
b
k
+
R
b
k
+
1
b
k
p
o
b
−
p
o
b
β
b
k
+
1
b
k
]
=
[
−
I
Δ
t
0
1
2
R
b
k
b
0
Δ
t
k
2
−
I
R
b
k
+
1
b
k
R
b
k
b
0
Δ
t
]
[
v
k
b
k
v
k
+
1
b
k
+
1
g
b
0
]
\begin{bmatrix} \alpha_{b_{k+1}}^{b_k}-\eta_{b_{k+1}}^{b_k}+R_{b_{k+1}}^{b_k}p_o^b-p_o^b\\ \beta_{b_{k+1}}^{b_k} \end{bmatrix}=\begin{bmatrix} -I\Delta t& 0& \frac{1}{2}R_{b_k}^{b_0}\Delta t_k^2 \\ -I& R_{b_{k+1}}^{b_k}& R_{b_k}^{b_0}\Delta t \end{bmatrix}\begin{bmatrix} v_k^{b_k}\\ v_{k+1}^{b_{k+1}}\\ g^{b_0} \end{bmatrix}
[αbk+1bk−ηbk+1bk+Rbk+1bkpob−pobβbk+1bk]=[−IΔt−I0Rbk+1bk21Rbkb0Δtk2Rbkb0Δt]⎣⎡vkbkvk+1bk+1gb0⎦⎤
3. 初始化尺度因子
R b 0 b k ( p o k + 1 b 0 − R b k + 1 b 0 p o b − p o k b 0 + R b k b 0 p o b ) = R c 0 b k ( s ( p ˉ b k + 1 c 0 − p ˉ b k c 0 ) ) = R c 0 b k ( s ( p ˉ c k + 1 c 0 − p ˉ c k c 0 ) − R b k + 1 c 0 p c b + R b k c 0 p c b ) \begin{aligned} R_{b_0}^{b_k}(p_{o_{k+1}}^{b_0}-R_{b_{k+1}}^{b_0}p_{o}^b-p_{o_{k}}^{b_0}+R_{b_k}^{b_0}p_{o}^b)&=R_{c_0}^{b_k}(s(\bar{p}_{b_{k+1}}^{c_0}-\bar{p}_{b_{k}}^{c_0}))\\&=R_{c_0}^{b_k}(s(\bar{p}_{c_{k+1}}^{c_0}-\bar{p}_{c_{k}}^{c_0})-R_{b_{k+1}}^{c_0}p_c^b+R_{b_{k}}^{c_0}p_c^b) \end{aligned} Rb0bk(pok+1b0−Rbk+1b0pob−pokb0+Rbkb0pob)=Rc0bk(s(pˉbk+1c0−pˉbkc0))=Rc0bk(s(pˉck+1c0−pˉckc0)−Rbk+1c0pcb+Rbkc0pcb)
4. 优化重力方向
利用球面坐标对三维的重力向量进行参数化,自由度变为2
g
b
0
=
∥
g
∥
⋅
g
b
0
+
w
1
b
1
+
w
2
b
2
g^{b_0}=\left \|g\right \|\cdot g^{b_0}+w_1{b_1}+w_2{b_2}
gb0=∥g∥⋅gb0+w1b1+w2b2
b
1
=
{
g
b
0
×
[
1
,
0
,
0
]
,
g
b
0
×
[
1
,
0
,
0
]
T
,
g
b
0
×
[
0
,
0
,
1
]
,
o
t
h
e
r
w
i
s
e
.
b
2
=
g
b
0
×
b
1
\begin{aligned} & b_{1}=\left\{ \begin{aligned} g^{b_0}\times \left [ 1, 0, 0 \right ] & , & g^{b_0}\times \left [ 1, 0, 0 \right ]^T, \\ g^{b_0}\times \left [ 0, 0, 1 \right ] & , & otherwise. \end{aligned} \right.\\ & b_2=g^{b_0}\times b_1 \end{aligned}
b1={gb0×[1,0,0]gb0×[0,0,1],,gb0×[1,0,0]T,otherwise.b2=gb0×b1
[
α
b
k
+
1
b
k
−
η
b
k
+
1
b
k
+
R
b
k
+
1
b
k
p
o
b
−
p
o
b
−
1
2
R
b
k
b
0
∥
g
∥
⋅
g
b
0
Δ
t
k
2
β
b
k
+
1
b
k
−
R
b
k
b
0
∥
g
∥
⋅
g
b
0
Δ
t
k
]
=
[
−
I
Δ
t
0
1
2
R
b
k
b
0
B
Δ
t
k
2
−
I
R
b
k
+
1
b
k
R
b
k
b
0
B
Δ
t
]
[
v
k
b
k
v
k
+
1
b
k
+
1
w
b
0
]
\begin{bmatrix} \alpha_{b_{k+1}}^{b_k}-\eta_{b_{k+1}}^{b_k}+R_{b_{k+1}}^{b_k}p_o^b-p_o^b-\frac{1}{2}R_{b_k}^{b_0}\left \|g\right \|\cdot g^{b_0}\Delta t_k^2 \\ \beta_{b_{k+1}}^{b_k}-R_{b_k}^{b_0}\left \|g\right \|\cdot g^{b_0}\Delta t_k \end{bmatrix}=\begin{bmatrix} -I\Delta t& 0& \frac{1}{2}R_{b_k}^{b_0}B\Delta t_k^2 \\ -I& R_{b_{k+1}}^{b_k}& R_{b_k}^{b_0}B\Delta t \end{bmatrix}\begin{bmatrix} v_k^{b_k}\\ v_{k+1}^{b_{k+1}}\\ w^{b_0} \end{bmatrix}
[αbk+1bk−ηbk+1bk+Rbk+1bkpob−pob−21Rbkb0∥g∥⋅gb0Δtk2βbk+1bk−Rbkb0∥g∥⋅gb0Δtk]=[−IΔt−I0Rbk+1bk21Rbkb0BΔtk2Rbkb0BΔt]⎣⎡vkbkvk+1bk+1wb0⎦⎤
实践
配合代码查看
具体对应代码在initial_alignment.cpp文件中(代码中注释的部分是源代码,相应的代码行是修改后的)
其中LinearAlignment
函数对应于初始化速度和重力
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x) { int all_frame_count = all_image_frame.size(); // int n_state = all_frame_count * 3 + 3 + 1; int n_state = all_frame_count * 3 + 3; MatrixXd A{n_state, n_state}; A.setZero(); VectorXd b{n_state}; b.setZero(); map<double, ImageFrame>::iterator frame_i; map<double, ImageFrame>::iterator frame_j; int i = 0; Eigen::Quaterniond q_b0bk; q_b0bk.setIdentity(); q_b0bk = q_b0bk * all_image_frame.begin()->second.pre_integration->delta_q; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) { frame_j = next(frame_i); // MatrixXd tmp_A(6, 10); MatrixXd tmp_A(6, 9); tmp_A.setZero(); VectorXd tmp_b(6); tmp_b.setZero(); double dt = frame_j->second.pre_integration->sum_dt; // tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); // tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity(); // tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0; // tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0]; // tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); // tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; // tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity(); // tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v; q_b0bk = q_b0bk * frame_j->second.pre_integration->delta_q; tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); tmp_A.block<3, 3>(0, 6) = 0.5 * frame_i->second.R.transpose() * RIC[0].transpose() * dt * dt * Matrix3d::Identity(); // tmp_A.block<3, 3>(0, 6) = 0.5 * q_b0bk.inverse().toRotationMatrix() * dt * dt * Matrix3d::Identity(); tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p - frame_j->second.pre_integration->delta_eta + frame_j->second.pre_integration->delta_q.toRotationMatrix() * TIO - TIO; tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); tmp_A.block<3, 3>(3, 3) = frame_i->second.pre_integration->delta_q.toRotationMatrix(); tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * RIC[0].transpose() * dt * Matrix3d::Identity(); // tmp_A.block<3, 3>(3, 6) = q_b0bk.inverse().toRotationMatrix() * dt * Matrix3d::Identity(); tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v; cout << "delta_v " << frame_j->second.pre_integration->delta_v.transpose() << endl; Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero(); //cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; //MatrixXd cov_inv = cov.inverse(); cov_inv.setIdentity(); MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>(); // A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>(); // b.tail<4>() += r_b.tail<4>(); // A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>(); // A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>(); A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>(); b.tail<3>() += r_b.tail<3>(); A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>(); A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>(); } A = A * 1000.0; b = b * 1000.0; VectorXd tmp_x; tmp_x = A.ldlt().solve(b); // double s = x(n_state - 1) / 100.0; // ROS_DEBUG("estimated scale: %f", s); // g = x.segment<3>(n_state - 4); g = tmp_x.segment<3>(n_state - 3); ROS_INFO_STREAM(" result g " << g.norm() << " " << g.transpose()); // if(fabs(g.norm() - G.norm()) > 1.0 || s < 0) // { // return false; // } if(fabs(g.norm() - G.norm()) > 1.0) { return false; } RefineGravity(all_image_frame, g, tmp_x); // 这里补充了尺度信息,保证外部接口调用的一致性 double s = solveScale(all_image_frame); x.resize(tmp_x.rows() + 1, tmp_x.cols()); x = tmp_x; (x.tail<1>())(0) = s; // s = (x.tail<1>())(0) / 100.0; // (x.tail<1>())(0) = s; g = RIC[0].transpose() * g; // 再转换到camera坐标系下 ROS_INFO_STREAM(" refine " << g.norm() << " " << g.transpose()); if(s < 0.0 ) return false; else return true; }
solveScale
函数对应于初始化尺度因子
// 估计初始尺度 double solveScale(map<double, ImageFrame> &all_image_frame) { MatrixXd A(1, 1); VectorXd b(1, 1); VectorXd s(1, 1); A.setZero(); b.setZero(); map<double, ImageFrame>::iterator frame_i; map<double, ImageFrame>::iterator frame_j; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++) { frame_j = next(frame_i); MatrixXd tmp_A(3, 1); tmp_A.setZero(); VectorXd tmp_b(3, 1); tmp_b.setZero(); tmp_A = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T); tmp_b = frame_j->second.pre_integration->delta_eta - frame_i->second.pre_integration->delta_q * TIO + TIO + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0]; A += tmp_A.transpose() * tmp_A; b += tmp_A.transpose() * tmp_b; } s = A.ldlt().solve(b); ROS_INFO_STREAM("A = " << A); ROS_INFO_STREAM("b = " << b); ROS_INFO("Visual scale %f", s[0]); return s[0]; }
RefineGravity
函数对应于优化重力方向
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x) { Vector3d g0 = g.normalized() * G.norm(); Vector3d lx, ly; //VectorXd x; int all_frame_count = all_image_frame.size(); // int n_state = all_frame_count * 3 + 2 + 1; int n_state = all_frame_count * 3 + 2; MatrixXd A{n_state, n_state}; A.setZero(); VectorXd b{n_state}; b.setZero(); map<double, ImageFrame>::iterator frame_i; map<double, ImageFrame>::iterator frame_j; for(int k = 0; k < 4; k++) { MatrixXd lxly(3, 2); lxly = TangentBasis(g0); int i = 0; Eigen::Quaterniond q_b0bk; q_b0bk.setIdentity(); q_b0bk = q_b0bk * all_image_frame.begin()->second.pre_integration->delta_q; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) { frame_j = next(frame_i); // MatrixXd tmp_A(6, 9); MatrixXd tmp_A(6, 8); tmp_A.setZero(); VectorXd tmp_b(6); tmp_b.setZero(); double dt = frame_j->second.pre_integration->sum_dt; // tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); // tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly; // tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0; // tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0; // tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); // tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; // tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly; // tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0; q_b0bk = q_b0bk * frame_j->second.pre_integration->delta_q; tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); tmp_A.block<3, 2>(0, 6) = 0.5 * frame_i->second.R.transpose() * RIC[0].transpose() * dt * dt * Matrix3d::Identity() * lxly; // tmp_A.block<3, 2>(0, 6) = 0.5 * q_b0bk.inverse().toRotationMatrix() * dt * dt * Matrix3d::Identity() * lxly; tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p - frame_j->second.pre_integration->delta_eta + frame_j->second.pre_integration->delta_q.toRotationMatrix() * TIO - TIO - 0.5 * frame_i->second.R.transpose() * RIC[0].transpose() * dt * dt * g0; // tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p - frame_j->second.pre_integration->delta_eta + // frame_j->second.pre_integration->delta_q.toRotationMatrix() * TIO - TIO - // 0.5 * q_b0bk.inverse().toRotationMatrix() * dt * dt * g0; tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); tmp_A.block<3, 3>(3, 3) = frame_i->second.pre_integration->delta_q.toRotationMatrix(); tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * RIC[0].transpose() * dt * Matrix3d::Identity() * lxly; // tmp_A.block<3, 2>(3, 6) = q_b0bk.inverse().toRotationMatrix() * dt * Matrix3d::Identity() * lxly; tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * RIC[0].transpose() * dt * Matrix3d::Identity() * g0; // tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - // q_b0bk.inverse().toRotationMatrix() * dt * Matrix3d::Identity() * g0; Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero(); //cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; //MatrixXd cov_inv = cov.inverse(); cov_inv.setIdentity(); MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>(); // A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>(); // b.tail<3>() += r_b.tail<3>(); // A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>(); // A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>(); A.bottomRightCorner<2, 2>() += r_A.bottomRightCorner<2, 2>(); b.tail<2>() += r_b.tail<2>(); A.block<6, 2>(i * 3, n_state - 2) += r_A.topRightCorner<6, 2>(); A.block<2, 6>(n_state - 3, i * 2) += r_A.bottomLeftCorner<2, 6>(); } A = A * 1000.0; b = b * 1000.0; x = A.ldlt().solve(b); // VectorXd dg = x.segment<2>(n_state - 3); VectorXd dg = x.segment<2>(n_state - 2); g0 = (g0 + lxly * dg).normalized() * G.norm(); //double s = x(n_state - 1); } g = g0; }
这篇关于VINS-Mono融合轮速计和GPS(二):鲁棒初始化的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!
- 2024-12-27OpenFeign服务间调用学习入门
- 2024-12-27OpenFeign服务间调用学习入门
- 2024-12-27OpenFeign学习入门:轻松掌握微服务通信
- 2024-12-27OpenFeign学习入门:轻松掌握微服务间的HTTP请求
- 2024-12-27JDK17新特性学习入门:简洁教程带你轻松上手
- 2024-12-27JMeter传递token学习入门教程
- 2024-12-27JMeter压测学习入门指南
- 2024-12-27JWT单点登录学习入门指南
- 2024-12-27JWT单点登录原理学习入门
- 2024-12-27JWT单点登录原理学习入门