项目地址VINS-GPS-Wheel,欢迎交流学习。
VINS-Mono融合轮速计和GPS(一):预积分
VINS-Mono融合轮速计和GPS(二):鲁棒初始化
VINS-Mono融合轮速计和GPS(三):优化和在线标定
VINS-Mono融合轮速计和GPS(四):融合GPS
上一篇博客介绍了带有轮速计的预积分相关内容(工作有变动,托挺久的,实在抱歉),本篇用预积分的结果对初始化过程进行优化。
先说一下VINS-Mono的初始化过程。首先,用SFM求解滑窗内所有帧的位姿,和所有路标点的3D位置,然后跟IMU预积分的值对齐,求解重力方向、尺度因子、陀螺仪bias及每一帧对应的速度。
流程如下:
考虑到IMU初始化较慢,估计视觉尺度效果也容易受影响,所以改进思路是采用轮速计的观测数据进行视觉尺度的估计和参与初始速度的估计。具体改动为,图中前三个步骤采用原框架的视觉初始化模块,visualInitialAlign是对visualIMUAlignment之后的结果进行对齐。所以改动主要在visualIMUAlignment模块。
将visualIMUAlignment详细展开来说,又分为4个步骤:
(和VINS-Mono相同,暂略,有空再加)
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⎦⎤
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)
利用球面坐标对三维的重力向量进行参数化,自由度变为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; }