lio-mapping及 VINS-Mono代码及理论推导（2）——pre_integration
lio-mapping 及 VINS-Mono代码及理论推导（2）—— pre_integration
最近在看港科⼤刚开源的LIO及VINS-Mono论⽂和代码，他们⼀部分代码都是相同的，闲暇时间整理⼀下。持续更新中（如果有时间）。。。
LIO
VINS
预积分部分核⼼函数为
MidPointIntegration(...)
MidPointIntegration 预积分函数
VINS_Mono
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0,const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1,const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p,const Eigen::Quaterniond &delta_q,const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba,const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg,bool update_jacobian)
{
//ROS_INFO("midpoint integration");
Vector3d un_acc_0 = delta_q *(_acc_0 - linearized_ba);
Vector3d un_gyr =0.5*(_gyr_0 + _gyr_1)- linearized_bg;
result_delta_q = delta_q *Quaterniond(1,un_gyr(0)* _dt /2,un_gyr(1)* _dt /2,un_gyr(2)* _dt /2);
Vector3d un_acc_1 = result_delta_q *(_acc_1 - linearized_ba);
Vector3d un_acc =0.5*(un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt +0.5* un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
if(update_jacobian)
{
Vector3d w_x =0.5*(_gyr_0 + _gyr_1)- linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<<0,-w_x(2),w_x(1),
w_x(2),0,-w_x(0),
-w_x(1),w_x(0),0;
R_a_0_x<<0,-a_0_x(2),a_0_x(1),
a_0_x(2),0,-a_0_x(0),
-a_0_x(1),a_0_x(0),0;
R_a_1_x<<0,-a_1_x(2),a_1_x(1),
a_1_x(2),0,-a_1_x(0),
-a_1_x(1),a_1_x(0),0;
MatrixXd F = MatrixXd::Zero(15,15);
F.block<3,3>(0,0)= Matrix3d::Identity();
F.block<3,3>(0,3)=-0.25* RotationMatrix()* R_a_0_x * _dt * _dt +
-0.25* result_RotationMatrix()* R_a_1_x *(Matrix3d::Identity()- R_w_x * _dt)* _dt * _dt;
F.block<3,3>(0,6)= MatrixXd::Identity(3,3)* _dt;
F.block<3,3>(0,9)=-0.25*(RotationMatrix()+ result_RotationMatrix())* _dt * _dt;
F.block<3,3>(0,12)=-0.25* result_RotationMatrix()* R_a_1_x * _dt * _dt *-_dt;
F.block<3,3>(3,3)= Matrix3d::Identity()- R_w_x * _dt;
F.block<3,3>(3,12)=-1.0* MatrixXd::Identity(3,3)* _dt;
F.block<3,3>(3,12)=-1.0* MatrixXd::Identity(3,3)* _dt;
F.block<3,3>(6,3)=-0.5* RotationMatrix()* R_a_0_x * _dt +
心无旁骛啥意思-0.5* result_RotationMatrix()* R_a_1_x *(Matrix3d::Identity()- R_w_x * _dt)* _dt;
F.block<3,3>(6,6)= Matrix3d::Identity();
F.block<3,3>(6,9)=-0.5*(RotationMatrix()+ result_RotationMatrix())* _dt;
F.block<3,3>(6,12)=-0.5* result_RotationMatrix()* R_a_1_x * _dt *-_dt;
F.block<3,3>(9,9)= Matrix3d::Identity();
F.block<3,3>(12,12)= Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3,3>(0,0)=0.25* RotationMatrix()* _dt * _dt;
V.block<3,3>(0,3)=0.25*-result_RotationMatrix()* R_a_1_x * _dt * _dt *0.5* _dt;
V.block<3,3>(0,6)=0.25* result_RotationMatrix()* _dt * _dt;
V.block<3,3>(0,9)= V.block<3,3>(0,3);
V.block<3,3>(3,3)=0.5* MatrixXd::Identity(3,3)* _dt;
V.block<3,3>(3,9)=0.5* MatrixXd::Identity(3,3)* _dt;
V.block<3,3>(6,0)=0.5* RotationMatrix()* _dt;
轻质泡沫混凝土V.block<3,3>(6,3)=0.5*-result_RotationMatrix()* R_a_1_x * _dt *0.5* _dt;
V.block<3,3>(6,6)=0.5* result_RotationMatrix()* _dt;
V.block<3,3>(6,9)= V.block<3,3>(6,3);
V.block<3,3>(9,12)= MatrixXd::Identity(3,3)* _dt;
V.block<3,3>(12,15)= MatrixXd::Identity(3,3)* _dt;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian;
covariance = F * covariance * F.transpo()+ V * noi * V.transpo();
}
}
with是介词吗
LIO-Mapping
对⽐代码发现 LIO-Mapping 与 VINS部分细节不同，注释中有提及。
void MidPointIntegration(double dt,
const Vector3d &acc0,const Vector3d &gyr0,
const Vector3d &acc1,const Vector3d &gyr1,
const Vector3d &delta_p,const Quaterniond &delta_q,
const Vector3d &delta_v,const Vector3d &linearized_ba,
const Vector3d &linearized_bg, Vector3d &result_delta_p,
Quaterniond &result_delta_q, Vector3d &result_delta_v,
Vector3d &result_linearized_ba, Vector3d &result_linearized_bg,
bool update_jacobian){
//ROS_DEBUG("midpoint integration");
// NOTE: the un_acc here is different from the un_acc in the Estimator
Vector3d un_acc_0 = delta_q *(acc0 - linearized_ba);
Vector3d un_gyr =0.5*(gyr0 + gyr1)- linearized_bg;
result_delta_q = delta_q *Quaterniond(1,un_gyr(0)* dt /2,un_gyr(1)* dt /2,un_gyr(2)* dt /2);
Vector3d un_acc_1 = result_delta_q *(acc1 - linearized_ba);
Vector3d un_acc =0.5*(un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * dt +0.5* un_acc * dt * dt;
result_delta_v = delta_v + un_acc * dt;
result_linearized_ba = linearized_ba;
苹果汤的功效
result_linearized_bg = linearized_bg;
if(update_jacobian){
Vector3d w_x =0.5*(gyr0 + gyr1)- linearized_bg;
Vector3d a_0_x = acc0 - linearized_ba;
Vector3d a_1_x = acc1 - linearized_ba;
成熟的句子
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x <<0,-w_x(2),w_x(1),
R_w_x << 0, -w_x (2), w_x (1), w_x (2), 0, -w_x (0), -w_x (1), w_x (0), 0;
R_a_0_x << 0, -a_0_x (2), a_0_x (1), a_0_x (2), 0, -a_0_x (0), -a_0_x (1), a_0_x (0), 0;
R_a_1_x << 0, -a_1_x (2), a_1_x (1), a_1_x (2), 0, -a_1_x (0), -a_1_x (1), a_1_x (0), 0; // NOTE: F = Fd = \Phi = I + dF*dt MatrixXd F = MatrixXd ::Zero (15, 15); F .block <3, 3>(0, 0) = Matrix3d ::Identity ();
F .block <3, 3>(0, 3) = -0.25 * delta_q .toRotationMatrix () * R_a_0_x * dt * dt +
-0.25 * result_delta_q .toRotationMatrix () * R_a_1_x * (Matrix3d ::Identity () - R_w_x * dt ) * dt * dt ; F .block <3, 3>(0, 6) = MatrixXd ::Identity (3, 3) * dt ;
F .block <3, 3>(0, 9) = -0.25 * (delta_q .toRotationMatrix () + result_delta_q .toRotationMatrix ()) * dt * dt ;// F.block<3, 3>(0, 12) = -0.25 * result_RotationMatrix() * R_a_1_x * dt * dt * -dt;
F .block <3, 3>(0, 12) = -0.1667 * result_delta_q .toRotationMatrix () * R_a_1_x * dt * dt * -dt ;//0.1667与VINS 不同 F .block <3, 3>(3, 3) = Matrix3d ::Identity () - R_w_x * dt ; F .block <3, 3>(3, 12) = -1.0 * MatrixXd ::Identity (3, 3) * dt ;
F .block <3, 3>(6, 3) = -0.5 * delta_q .toRotationMatrix () * R_a_0_x * dt +
-0.5 * result_delta_q .toRotationMatrix () * R_a_1_x * (Matrix3d ::Identity () - R_w_x * dt ) * dt ; F .block <3, 3>(6, 6) = Matrix3d ::Identity ();
F .block <3, 3>(6, 9) = -0.5 * (delta_q .toRotationMatrix () + result_delta_q .toRotationMatrix ()) * dt ; F .block <3, 3>(6, 12) = -0.5 * result_delta_q .toRotationMatrix () * R_a_1_x * dt * -dt ; F .block <3, 3>(9, 9) = Matrix3d ::Identity (); F .block <3, 3>(12, 12) = Matrix3d ::Identity (); //cout<<"A"<<endl<<A<<endl; // NOTE: V = Fd * G_c
积极向上的网名// FIXME: verify if it is right, the 0.25 part MatrixXd V = MatrixXd ::Zero (15, 18);
// V.block<3, 3>(0, 0) = 0.25 * RotationMatrix() * dt * dt;
V .block <3, 3>(0, 0) = 0.5 * delta_q .toRotationMatrix () * dt * dt ;//0.5与VINS 不同
V .block <3, 3>(0, 3) = 0.25 * -result_delta_q .toRotationMatrix () * R_a_1_x * dt * dt * 0.5 * dt ;// V.block<3, 3>(0, 6) = 0.25 * result_RotationMatrix() * dt * dt;
V .block <3, 3>(0, 6) = 0.5 * result_delta_q .toRotationMatrix () * dt * dt ;//0.5与VINS 不同 V .block <3, 3>(0, 9) = V .block <3, 3>(0, 3);
V .block <3, 3>(3, 3) = 0.5 * MatrixXd ::Identity (3, 3) * dt ; V .block <3, 3>(3, 9) = 0.5 * MatrixXd ::Identity (3, 3) * dt ; V .block <3, 3>(6, 0) = 0.5 * delta_q .toRotationMatrix () * dt ;
王者匹配机制V .block <3, 3>(6, 3) = 0.5 * -result_delta_q .toRotationMatrix () * R_a_1_x * dt * 0.5 * dt ; V .block <3, 3>(6, 6) = 0.5 * result_delta_q .toRotationMatrix () * dt ; V .block <3, 3>(6, 9) = V .block <3, 3>(6, 3);
V .block <3, 3>(9, 12) = MatrixXd ::Identity (3, 3) * dt ; V .block <3, 3>(12, 15) = MatrixXd ::Identity (3, 3) * dt ; //step_jacobian = F; //step_V = V;
jacobian_ = F * jacobian_;
covariance_ = F * covariance_ * F .transpo () + V * noi_ * V .transpo (); } }
公式推导
下⾯是两帧雷达数据之间所以IMU数据的位置、速度、旋转量的积分公式
p =j p +i [v Δt +k =i ∑
j −1
k 0.25(R (a −k i
k b )+a k R (a −k +1i k b ))Δt ]
a k 2(1)
不考虑噪声的情况下，每来⼀帧imu数据，其状态转移如下
误差状态加⼊噪声影响，加速度和陀螺仪的测量噪声为 ，。 加速度和陀螺仪偏置为随机游⾛噪声， ， ，针对MidPointIntegration，其还需要增加时刻的测量噪声，也即其噪声矩阵为。
.
v =j v +
i 0.5[R (a −k =i ∑
j −1
k i
k b )Δt +a k R (a −k +1i
k b )Δt ]
a k (2)
q =j q ⊗
i δq =k =i ∏
j −1
刘备摔孩子
k q ⊗
i k =i ∏
j −1
[
Δt (−b )
21
2
ω+ωk k +1g k 1
]
(3)
q =k +1q ⊗k [
Δt (−b )
21
2
ω+ωk k +1g k 1
]
(4)
p =k +1p +k v Δt +k 0.25(R (a −k k b )+a k R (a −k +1k +1b ))Δt a k +12
(5)
v =k +1v +k 0.5(R (a −k k b )+a k R (a −k +1k +1b ))Δt a k +1(6)
b =a k +1b a k (7)
b =g k +1b g k
(8)
δX =(δp ,δθ,δv ,δb ,δb )
a g n ∼a N (0,σ)a 2
n ∼w N (0,σ)w 2
n ∼
b a N (0,σ)b a 2
n ∼b w N (0,σ)b w 2
k +1Q =⎣⎢⎢⎢⎢⎢⎢⎡n a 000000n w 000000n a 0000
0n w 00
0000n b a 000000n b w ⎦⎥⎥⎥⎥⎥⎥⎤
根据式(4)，⾸先求，定义：更新为：
假设加加速度恒定，再求 :
得到:
再求求最后更新偏置,δθ
˙δq =[δθ
211]=δθ˙−[−2
ω+ωk k +1
b ]δθ−g k ×δb +g 0.5(n +w k n ) w k +1(9)
δθδθ←new (I −[(−2
ω+ωk k +1
b )]Δt )δθ−g k ×Δt δb +g 0.5Δt (n +w k n )w k +1(10)
δv ˙=δv ˙==−0.5(R [a −b ]δθ+R [a −b ]δθ)
k k a k ×k +1k +1a k +1×new +0.5(R +R )δb +0.5(R n +R n )
k k +1a k a k k +1a k +1−0.5(R [a −b ]δθ+R [a −b ]((I −[(−b )]Δt )δθ−Δt δb +0.5Δt (n +n ))k k a k ×k +1k +1a k +1×2ω+ωk k +1
g k ×g w k w k +1+0.5(R +R )δb +0.5(R n +R n )
k k +1a k a k k +1a k +1−0.5(R [a −b ]+R [a −b ]((I −[(−b )]Δt ))δθk k a k ×k +1k +1a k +1×2ω+ωk k +1g k ×−0.5(R +R )δb +0.5(R n +R n )
k k +1a k a k k +1a k +1+0.5R [a −b ]Δt δb −0.25R [a −b ]Δt (n +n )k +1k +1a k +1×g k +1k +1a k +1×w k w k +1(11)
δv new δv ←new δv +Δt
δv ˙(12)
δp
˙=δp ˙=0.5(δv +δv )new δv +0.5δv
˙(13)
δp
δp ←new δp +Δt
δp ˙(14)
δb a δb g
δb ←a
new
δb +a n Δt b a (15)
δb ←g
new
δb +g n Δt
b w (16)
