LIO 代码及理论推导（1）—— ImuInitialization
最近在看港科大刚开源的LIO论文和代码，闲暇时间整理一下。持续更新中（如果有时间）。。。
代码链接：https://github.com/hyye/lio-mapping
相关介绍：https://sites.google.com/view/lio-mapping
论文地址：https://arxiv.org/abs/1904.06993
IMU初始化主要设计三个函数
EstimateGyroBias(...) //估计陀螺仪偏置
ApproximateGravity(...)//估计重力
RefineGravityAccBias(...)//refine重力和每个节点速度
EstimateExtrinsicRotation(...)//估计imu与LIDAR之间的旋转
EstimateGyroBias部分比较简单这里不做整理。
ApproximateGravity 估计重力
代码
bool ApproximateGravity(CircularBuffer<PairTimeLaserTransform> &all_laser_transforms, Vector3d &g,
Transform &transform_lb) {
size_t num_states = 3;
size_t window_size = all_laser_transforms.size() - 1;
if (window_size < 5) {
DLOG(WARNING) << ">>>>>>> window size not enough <<<<<<<";
return false;
}
Matrix3d I3x3;
I3x3.setIdentity();
MatrixXd A{num_states, num_states};
A.setZero();
VectorXd b{num_states};
b.setZero();
Vector3d &g_approx = g;
for (size_t i = 0; i < window_size - 1; ++i) {
PairTimeLaserTransform &laser_trans_i = all_laser_transforms[i];
PairTimeLaserTransform &laser_trans_j = all_laser_transforms[i + 1];
PairTimeLaserTransform &laser_trans_k = all_laser_transforms[i + 2];
double dt12 = laser_trans_j.second.pre_integration->sum_dt_;
double dt23 = laser_trans_k.second.pre_integration->sum_dt_;
Vector3d dp12 = laser_trans_j.second.pre_integration->delta_p_.template cast<double>();
Vector3d dp23 = laser_trans_k.second.pre_integration->delta_p_.template cast<double>();
Vector3d dv12 = laser_trans_j.second.pre_integration->delta_v_.template cast<double>();
Vector3d pl1 = laser_trans_i.second.transform.pos.template cast<double>();
Vector3d pl2 = laser_trans_j.second.transform.pos.template cast<double>();
Vector3d pl3 = laser_trans_k.second.transform.pos.template cast<double>();
Vector3d plb = transform_lb.pos.template cast<double>();
Matrix3d rl1 = laser_trans_i.second.transform.rot.template cast<double>().toRotationMatrix();
Matrix3d rl2 = laser_trans_j.second.transform.rot.template cast<double>().toRotationMatrix();
Matrix3d rl3 = laser_trans_k.second.transform.rot.template cast<double>().toRotationMatrix();
Matrix3d rlb = transform_lb.rot.template cast<double>().toRotationMatrix();
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
//tmp_A = 0.5 * I3x3 * (2*(dt12 * dt12 * dt23)/*dv*/ - (dt12 * dt12 * dt23)/*dp*/ + (dt23 * dt23 * dt12)/*dp*/)
tmp_A = 0.5 * I3x3 * (dt12 * dt12 * dt23 + dt23 * dt23 * dt12);
tmp_b = (pl2 - pl1) * dt23 - (pl3 - pl2) * dt12
+ (rl2 - rl1) * plb * dt23 - (rl3 - rl2) * plb * dt12
+ rl2 * rlb * dp23 * dt12 + rl1 * rlb * dv12 * dt12 * dt23
- rl1 * rlb * dp12 * dt23;
A += tmp_A.transpose() * tmp_A;
b -= tmp_A.transpose() * tmp_b;
// A += tmp_A;
// b -= tmp_b;
}
A = A * 10000.0;
b = b * 10000.0;
// DLOG(INFO) << "A" << endl << A;
// DLOG(INFO) << "b" << endl << b;
g_approx = A.ldlt().solve(b);
// g_approx = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
LOG(INFO) << "g_approx: " << g_approx.transpose();
// TODO: verify g
double g_norm = all_laser_transforms.first().second.pre_integration->config_.g_norm;
return fabs(g_approx.norm() - g_norm) <= 1.0;
}
(1) d P i j = ∑ k = i j − 1 [ v k Δ t + 0.5 R k i ( a k − b a k ) Δ t 2 ] dP_{ij}=\sum_{k=i}^{j-1}[v_k\Delta{t}+0.5R_k^i(a_k-b_{a_k})\Delta{t}^2] \tag{1} dPij=k=i∑j−1[vkΔt+0.5Rki(ak−bak)Δt2](1)
(2) d V i j = ∑ k = i j − 1 [ R k i ( a k − b a k ) Δ t ] dV_{ij}=\sum_{k=i}^{j-1}[R_k^i(a_k-b_{a_k})\Delta{t}] \tag{2} dVij=k=i∑j−1[Rki(ak−bak)Δt](2)
(3) q j = q i ⊗ ∏ k = i j − 1 δ q k = q i ⊗ ∏ k = i j − 1 [ 1 2 Δ t ( ω k − b g k ) 1 ] q_j=q_i\otimes\prod_{k=i}^{j-1}\delta{q_k}=q_i\otimes\prod_{k=i}^{j-1}\begin{bmatrix}\frac{1}{2}\Delta{t}(\omega_k-b_{g_k}) \\ 1 \\ \end{bmatrix} \tag{3} qj=qi⊗k=i∏j−1δqk=qi⊗k=i∏j−1[21Δt(ωk−bgk)1](3)
(4) P 2 − P 1 = V 1 t 12 + R 1 d P 12 + 0.5 g t 12 2 P_2-P_1=V_1t_{12}+R_1dP_{12}+0.5gt_{12}^2 \tag{4} P2−P1=V1t12+R1dP12+0.5gt122(4)
(5) P 3 − P 2 = V 2 t 23 + R 2 d P 23 + 0.5 g t 23 2 P_3-P_2=V_2t_{23}+R_2dP_{23}+0.5gt_{23}^2 \tag{5} P3−P2=V2t23+R2dP23+0.5gt232(5)
(6) V 2 = V 1 + R 1 d V 12 + g t 12 V_2=V_1+R_1dV_{12}+gt_{12} \tag{6} V2=V1+R1dV12+gt12(6)
公式 ( 1 ) ( 2 ) ( 3 ) (1)(2)(3) (1)(2)(3) 为imu第i帧到第j帧之间的预积分。这一块的代码在预积分部分。
公式 ( 4 ) ( 5 ) ( 6 ) (4)(5)(6) (4)(5)(6) 可以推导出代码中的误差和雅克比矩阵计算过程。其中 P i P_i Pi代表雷达里程计获得的位置 d P i j dP_{ij} dPij则代表imu预积分的结果，为方便表示，这两种位置均转换到了雷达坐标系下（推到过程省略了，代码中有）。
(
5
)
t
12
+
(
6
)
t
12
t
23
−
(
4
)
t
23
(5)t_{12}+(6)t_{12}t_{23}-(4)t_{23}
(5)t12+(6)t12t23−(4)t23可得到
(7)
(
P
3
−
P
2
)
t
12
−
(
P
2
−
P
1
)
t
23
=
R
2
d
P
23
t
12
−
R
1
d
P
12
t
23
+
R
1
d
V
12
t
12
t
23
+
0.5
g
(
t
12
2
t
23
+
t
23
2
t
12
)
(P_3-P_2)t_{12}-(P_2-P_1)t_{23}=R_2dP_{23}t_{12} - R_1dP_{12}t_{23}+ R_1dV_{12}t_{12}t_{23}+0.5g(t_{12}^2t_{23}+t_{23}^2t_{12}) \tag{7}
(P3−P2)t12−(P2−P1)t23=R2dP23t12−R1dP12t23+R1dV12t12t23+0.5g(t122t23+t232t12)(7)
整理成
A
X
=
b
AX=b
AX=b形式:
(8)
0.5
(
t
12
2
t
23
+
t
23
2
t
12
)
g
=
−
(
(
P
2
−
P
1
)
t
23
−
(
P
3
−
P
2
)
t
12
+
R
2
d
P
23
t
12
−
R
1
d
P
12
t
23
+
R
1
d
V
12
t
12
t
23
)
0.5(t_{12}^2t_{23}+t_{23}^2t_{12})g = -((P_2-P_1)t_{23} - (P_3-P_2)t_{12}+R_2dP_{23}t_{12} - R_1dP_{12}t_{23}+ R_1dV_{12}t_{12}t_{23}) \tag{8}
0.5(t122t23+t232t12)g=−((P2−P1)t23−(P3−P2)t12+R2dP23t12−R1dP12t23+R1dV12t12t23)(8)
RefineGravityAccBias refine重力和每个节点速度
void RefineGravityAccBias(CircularBuffer<PairTimeLaserTransform> &all_laser_transforms,
CircularBuffer<Vector3d> &Vs,
CircularBuffer<Vector3d> &Bgs,
Vector3d &g_approx,
Transform &transform_lb,
Matrix3d &R_WI) {
typedef Sophus::SO3d SO3;
Vector3d &g_refined = g_approx;
size_t size_velocities = all_laser_transforms.size();
size_t num_states = size_velocities * 3 + 2;
LOG_ASSERT(size_velocities >= 5) << ">>>>>>> window size not enough <<<<<<<";
Matrix3d I3x3;
I3x3.setIdentity();
MatrixXd A{num_states, num_states};
A.setZero();
VectorXd b{num_states};
b.setZero();
VectorXd x{num_states};
x.setZero();
double g_norm = all_laser_transforms.first().second.pre_integration->config_.g_norm;
// Vector3d gI_n{0.0, 0.0, -1.0};
// Vector3d gW_n = g_refined.normalized(); // NOTE: the Lidar's world frame
// Vector3d gIxgW = gI_n.cross(gW_n);
// Vector3d v_WI = gIxgW / gIxgW.norm();
// double ang_WI = atan2(gIxgW.norm(), gI_n.dot(gW_n));
//
// Eigen::Matrix3d r_WI(SO3::exp(ang_WI * v_WI).unit_quaternion());
// DLOG(INFO) << (r_WI * gI_n * g_approx.norm()).transpose();
// DLOG(INFO) << g_approx.transpose();
g_refined = g_refined.normalized() * g_norm;
for (int k = 0; k < 5; ++k) {
MatrixXd lxly(3, 2);
lxly = TangentBasis(g_refined);
for (size_t i = 0; i < size_velocities - 1; ++i) {
PairTimeLaserTransform &laser_trans_i = all_laser_transforms[i];
PairTimeLaserTransform &laser_trans_j = all_laser_transforms[i + 1];
MatrixXd tmp_A(6, 8);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt12 = laser_trans_j.second.pre_integration->sum_dt_;
Vector3d dp12 = laser_trans_j.second.pre_integration->delta_p_;
Vector3d dv12 = laser_trans_j.second.pre_integration->delta_v_;
Vector3d pl1 = laser_trans_i.second.transform.pos.template cast<double>();
Vector3d pl2 = laser_trans_j.second.transform.pos.template cast<double>();
Vector3d plb = transform_lb.pos.template cast<double>();
Matrix3d rl1 = laser_trans_i.second.transform.rot.normalized().template cast<double>().toRotationMatrix();
Matrix3d rl2 = laser_trans_j.second.transform.rot.normalized().template cast<double>().toRotationMatrix();
Matrix3d rlb = transform_lb.rot.normalized().template cast<double>().toRotationMatrix();
tmp_A.block<3, 3>(0, 0) = dt12 * Matrix3d::Identity();
// tmp_A.block<3, 2>(0, 6) = (-0.5 * r_WI * SO3::hat(gI_n) * g_norm * dt12 * dt12).topLeftCorner<3, 2>();
// tmp_b.block<3, 1>(0, 0) =
// pl2 - pl1 - rl1 * rlb * dp12 - (rl1 - rl2) * plb - 0.5 * r_WI * gI_n * g_norm * dt12 * dt12;
tmp_A.block<3, 2>(0, 6) = 0.5 * Matrix3d::Identity() * lxly * dt12 * dt12;
tmp_b.block<3, 1>(0, 0) =
pl2 - pl1 - rl1 * rlb * dp12 - (rl1 - rl2) * plb - 0.5 * g_refined * dt12 * dt12;
tmp_A.block<3, 3>(3, 0) = Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = -Matrix3d::Identity();
// tmp_A.block<3, 2>(3, 6) = (-r_WI * SO3::hat(gI_n) * g_norm * dt12).topLeftCorner<3, 2>();
// tmp_b.block<3, 1>(3, 0) = -rl1 * rlb * dv12 - r_WI * gI_n * g_norm * dt12;
tmp_A.block<3, 2>(3, 6) = Matrix3d::Identity() * lxly * dt12;
tmp_b.block<3, 1>(3, 0) = -rl1 * rlb * dv12 - g_refined * dt12;
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<2, 2>() += r_A.bottomRightCorner<2, 2>();
b.tail<2>() += r_b.tail<2>();
A.block<6, 2>(i * 3, num_states - 2) += r_A.topRightCorner<6, 2>();
A.block<2, 6>(num_states - 2, i * 3) += r_A.bottomLeftCorner<2, 6>();
}
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b);
DLOG(INFO) << "k: " << k << ", x: " << x.transpose();
// TODO: verify if the gravity is right
Eigen::Vector2d dg = x.segment<2>(num_states - 2);
DLOG(INFO) << "dg: " << dg.x() << ", " << dg.y();
// R_WI = r_WI * SO3::exp(Vector3d(dg.x(), dg.y(), 0.0)).unit_quaternion();
// // WARNING
// r_WI = R_WI;
// g_refined = R_WI * gI_n * g_norm;
g_refined = (g_refined + lxly * dg).normalized() * g_norm;
}
Vector3d gI_n{0.0, 0.0, -1.0};
Vector3d gW_n = g_refined.normalized(); // NOTE: the Lidar's world frame
Vector3d gIxgW = gI_n.cross(gW_n);
Vector3d v_WI = gIxgW / gIxgW.norm();
double ang_WI = atan2(gIxgW.norm(), gI_n.dot(gW_n));
Eigen::Matrix3d r_WI(SO3::exp(ang_WI * v_WI).unit_quaternion());
R_WI = r_WI;
// WARNING check velocity
for (int i = 0; i < size_velocities; ++i) {
Vs[i] = x.segment<3>(i * 3);
DLOG(INFO) << "Vs[" << i << "]" << Vs[i].transpose();
}
}
代码中重要部分即是求 A 、 b A、b A、b矩阵。
建立两个相邻时刻位置和速度关系:
(9)
P
2
=
P
1
+
R
1
d
P
12
+
V
1
t
12
+
0.5
g
t
12
2
+
0.5
G
x
y
d
g
t
12
2
P_2=P_1+R_1dP_{12}+V_1t_{12}+0.5gt_{12}^2+0.5G_{xy}dgt_{12}^2 \tag{9}
P2=P1+R1dP12+V1t12+0.5gt122+0.5Gxydgt122(9)
(10) V 2 = V 1 + R 1 d V 12 + g t 12 + G x y d g t 12 V_2=V_1+R_1dV_{12}+gt_{12}+G_{xy}dgt_{12} \tag{10} V2=V1+R1dV12+gt12+Gxydgt12(10)
其中 d g dg dg代表xy方向上重力分量的增量， G x y G_{xy} Gxy代表将xy方向上重力分量的增量转换为三维形式，代码中用 l x l y lxly lxly表示。其他变量意义与公式 ( 4 ) ( 5 ) ( 6 ) (4)(5)(6) (4)(5)(6) 中一致。
其中
V
1
、
V
2
、
d
g
V_1、V_2、dg
V1、V2、dg为待估计的状态变量，将公式
(
9
)
(
10
)
(9)(10)
(9)(10) 整理为
A
X
=
b
AX=b
AX=b形式：
(11)
[
t
12
0
0.5
G
x
y
t
12
2
1
−
1
G
x
y
t
12
]
[
V
1
V
2
d
g
]
=
[
P
2
−
P
1
−
R
1
d
P
12
−
V
1
t
12
−
0.5
g
t
12
2
−
R
1
d
V
12
−
g
t
12
]
\begin{bmatrix} t_{12} & 0 & 0.5G_{xy}t_{12}^2\\ 1 & -1 & G_{xy}t_{12} \end{bmatrix} \begin{bmatrix} V_1\\ V_2\\ dg \end{bmatrix}= \begin{bmatrix} P_2-P_1-R_1dP_{12}-V_1t_{12}-0.5gt_{12}^2\\ -R_1dV_{12}-gt_{12} \end{bmatrix} \tag{11}
[t1210−10.5Gxyt122Gxyt12]⎣⎡V1V2dg⎦⎤=[P2−P1−R1dP12−V1t12−0.5gt122−R1dV12−gt12](11)
上述只有两个相邻时刻状态的关系，将所有时刻的状态矩阵
A
T
A
A^TA
ATA及
A
T
b
A^Tb
ATb相加得到
H
H
H及
g
g
g矩阵，最后求解:
H
X
=
g
HX=g
HX=g
得到每一时刻的速度及重力增量(在xy方向上的)。
EstimateExtrinsicRotation 估计imu与LIDAR之间的旋转
按照惯例先贴代码
bool ImuInitializer::EstimateExtrinsicRotation(CircularBuffer<PairTimeLaserTransform> &all_laser_transforms,
Transform &transform_lb) {
Transform transform_bl = transform_lb.inverse();
Eigen::Quaterniond rot_bl = transform_bl.rot.template cast<double>();
size_t window_size = all_laser_transforms.size() - 1;
Eigen::MatrixXd A(window_size * 4, 4);
for (size_t i = 0; i < window_size; ++i) {
PairTimeLaserTransform &laser_trans_i = all_laser_transforms[i];
PairTimeLaserTransform &laser_trans_j = all_laser_transforms[i + 1];
Eigen::Quaterniond delta_qij_imu = laser_trans_j.second.pre_integration->delta_q_;
Eigen::Quaterniond delta_qij_laser
= (laser_trans_i.second.transform.rot.conjugate() * laser_trans_j.second.transform.rot).template cast<double>();
Eigen::Quaterniond delta_qij_laser_from_imu = rot_bl.conjugate() * delta_qij_imu * rot_bl;
double angular_distance = 180 / M_PI * delta_qij_laser.angularDistance(delta_qij_laser_from_imu);
// DLOG(INFO) << i << ", " << angular_distance;
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
Eigen::Matrix4d lq_mat = LeftQuatMatrix(delta_qij_laser);
Eigen::Matrix4d rq_mat = RightQuatMatrix(delta_qij_imu);
A.block<4, 4>(i * 4, 0) = huber * (lq_mat - rq_mat);//? can't understand ?
// cout << (lq_mat * transform_lb.rot.coeffs().template cast<double>()).transpose() << endl;
//
// cout << (delta_qij_laser * transform_lb.rot.template cast<double>()).coeffs().transpose() << endl;
//
// cout << (rq_mat * transform_lb.rot.coeffs().template cast<double>()).transpose() << endl;
//
// cout << (transform_lb.rot.template cast<double>() * delta_qij_imu).coeffs().transpose() << endl;
}
// DLOG(INFO) << ">>>>>>> A <<<<<<<" << endl << A;
Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_qlb(x);
transform_lb.rot = estimated_qlb.cast<float>().toRotationMatrix();
Vector3d cov = svd.singularValues().tail<3>();
// DLOG(INFO) << "x: " << x.transpose();
DLOG(INFO) << "extrinsic rotation: " << transform_lb.rot.coeffs().transpose();
// cout << x.transpose << endl;
// DLOG(INFO) << "singular values: " << svd.singularValues().transpose();
// cout << cov << endl;
// NOTE: covariance 0.25
if (cov(1) > 0.25) {
return true;
} else {
return false;
}
}
i
j
ij
ij时刻雷达坐标系、IMU坐标系的旋转及两个坐标系外参的关系如下：
(12)
L
q
j
i
⊗
b
L
q
=
b
L
q
⊗
b
q
j
i
{}^Lq^i_j\otimes{}^L_bq={}^L_bq\otimes{}^bq^i_j \tag{12}
Lqji⊗bLq=bLq⊗bqji(12)
将四元数乘法，通过左四元数乘法矩阵
[
]
L
[ ]_L
[ ]L、右四元数乘法矩阵
[
]
R
[ ]_R
[ ]R转换为点乘运算：
(13)
[
L
q
j
i
]
L
b
L
q
=
[
b
q
j
i
]
R
b
L
q
[{}^Lq^i_j]_L{}^L_bq=[{}^bq^i_j]_R{}^L_bq \tag{13}
[Lqji]LbLq=[bqji]RbLq(13)
整理得到
(14)
(
[
L
q
j
i
]
L
−
[
b
q
j
i
]
R
)
b
L
q
=
0
([{}^Lq^i_j]_L-[{}^bq^i_j]_R){}^L_bq =0 \tag{14}
([Lqji]L−[bqji]R)bLq=0(14)
设
A
=
[
L
q
j
i
]
L
−
[
b
q
j
i
]
R
A=[{}^Lq^i_j]_L-[{}^bq^i_j]_R
A=[Lqji]L−[bqji]R ,利用SVD分解
(
A
=
U
S
V
T
)
(A=USV^T)
(A=USVT)求解上述齐次线性方程：
A A A为 4 k × 4 4k\times4 4k×4维矩阵， U U U为 4 k × 4 k 4k\times4k 4k×4k维矩阵， S S S为 4 k × 4 4k\times4 4k×4维矩阵， V V V为 4 × 4 4\times4 4×4维矩阵， k k k为节点数。 S = [ Σ 0 ] S=\begin{bmatrix} \Sigma\\0 \end{bmatrix} S=[Σ0], Σ \Sigma Σ为对角矩阵， 4 × 4 4\times4 4×4维，且从左到右依次变小。
因为 V V V矩阵为正交矩阵，也即它的每一列都正交， Σ \Sigma Σ的最后一个值最小，因此我们只要 b L q {}^L_bq bLq 等于 V V V矩阵最后一列（与 Σ \Sigma Σ最后一个值对应），即可保证方程左边的结果最接近０(上三行因为向量正交的缘故为０，而最后一行对应的 Σ \Sigma Σ最小)．
5565
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
