gtsam关于imu预积分实现探究
资料
gtsam官网关于IMU预积分的资料介绍了gtsam预积分基于以下论文实现
Todd Lupton and Salah Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions”, TRO, 28(1):61-76, 2012.
同时基于流形空间进行了优化,基于以下两篇论文
Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, “Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors”, Int. Conf. on Robotics and Automation (ICRA), 2014.
Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, “IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation”, Robotics: Science and Systems (RSS), 2015.
代码分析
从main文件着手
我们从gtsam给的例程代码入手,ImuFactorsExample.cpp
文件描述了IMU预积分GPS进行导航的代码。
打开输入输出文件以后,首先是基础操作初始化一个非线性因子图
// Add all prior factors (pose, velocity, bias) to the graph.
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
例程提供了两种预积分方式PreintegratedCombinedMeasurements
和PreintegratedImuMeasurements
,我们按PreintegratedImuMeasurements
进行分析。
在初始化一个IMU预积分量之前首先初始化了预积分的基础参数,各种协方差
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
// PreintegrationBase params:
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
// should be using 2nd order integration
// PreintegratedRotation params:
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
初始化一个imu预积分对象
imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
读取数据,如果是imu数据,就进行预积分
if (type == 0) { // IMU measurement
Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
for (int i=0; i<5; ++i) {
getlie(file, value, ',');
imu(i) = atof(value.c_str());
}
getline(file, value, '\n');
imu(5) = atof(value.c_str());
// Adding the IMU preintegration.
imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
}
追踪imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt)
,可以发现函数定义为
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt) {
// NOTE(frank): integrateMeasurement always needs to compute the derivatives,
// even when not of interest to the caller. Provide scratch space here.
Matrix9 A;
Matrix93 B, C;
update(measuredAcc, measuredOmega, dt, &A, &B, &C);
}
核心为update(measuredAcc, measuredOmega, dt, &A, &B, &C);
,关于这个updata
函数gtsam有两种实现,TangentPreintegration::update
和ManifoldPreintegration::update
。即基于切空间和基于流形空间的实现。选用哪种实现取决于gtsam编译安装时GTSAM_TANGENT_PREINTEGRATION.
为ON
还是OFF
。默认为切空间;流形空间的实现基于RSS2015的文献实现,切空间的实现在gtsam自带的文档有介绍。这里我们以默认的切空间分析。
切空间的更新函数实现
void TangentPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Do update
deltaTij_ += dt;
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc
// where acc_H_biasAcc = -I_3x3, hence
// new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
// new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega
// where omega_H_biasOmega = -I_3x3, hence
// new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
在这个函数中,首先对加速度和角速度进行修正,减去当前其偏置值(bias),得到修正后的加速度角速度:
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
body坐标系和imu坐标系之间的转换
默认情况下imu坐标系和body坐标系是一个坐标系,也可以设置不是一个坐标系。
通过在初始化时设置p->body_P_sensor
的值确定imu坐标系到body坐标系的转换矩阵。如果设置了两个坐标系之间的转换关系,那么需要将imu测量的imu坐标系的加速度和角速度转换为body坐标系的加速度角速度。在updata()
函数的中的这几行代码就是执行这一操作:
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
代码
追踪correctMeasurementsBySensorPose()
函数我们可以看到这个函数的实现:
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega) const {
assert(p().body_P_sensor);
// Compensate for sensor-body displacement if needed: we express the quantities
// (originally in the IMU frame) into the body frame
// Equations below assume the "body" frame is the CG
// Get sensor to body rotation matrix
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
// Convert angular velocity and acceleration from sensor to body frame
Vector3 correctedAcc = bRs * unbiasedAcc;
const Vector3 correctedOmega = bRs * unbiasedOmega;
// Jacobians
if (correctedAcc_H_unbiasedAcc) *correctedAcc_H_unbiasedAcc = bRs;
if (correctedAcc_H_unbiasedOmega) *correctedAcc_H_unbiasedOmega = Z_3x3;
if (correctedOmega_H_unbiasedOmega) *correctedOmega_H_unbiasedOmega = bRs;
// Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation();
if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the unbiased one
// to get linear acceleration vector in the body frame:
const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
correctedAcc -= body_Omega_body * b_velocity_bs;
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (correctedAcc_H_unbiasedOmega) {
double wdp = correctedOmega.dot(b_arm);
const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal();
*correctedAcc_H_unbiasedOmega = -( diag_wdp
+ correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * unbiasedOmega.transpose();
}
}
return make_pair(correctedAcc, correctedOmega);
}
公式化表达
速度加速度从sensor(IMU)坐标系转换到body坐标系
ω
′
=
R
s
b
ω
\omega^\prime=R_s^b \omega
ω′=Rsbω
a
′
=
R
s
b
a
a^\prime=R_s^b a
a′=Rsba
离心加速度
v
b
s
b
=
ω
′
×
p
v_{bs}^b=\omega^\prime\times p
vbsb=ω′×p
a
c
=
ω
′
×
v
b
s
b
a_c=\omega^\prime\times v_{bs}^b
ac=ω′×vbsb
将离心加速度从加速中去掉
a
′
=
a
′
−
a
c
a\prime=a\prime-a_c
a′=a′−ac
更新预积分量
下面是重头戏,更新预积分量了。
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
从函数调用可以看到,函数接收7个值,刚才修正过后的加速度,角速度,时间间隔,上一次运算结束保存的预积分量,A,B,C。
追踪这个函数,我们先看一下整体实现。
代码
Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt, const Vector9& preintegrated,
OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) {
const auto theta = preintegrated.segment<3>(0);
const auto position = preintegrated.segment<3>(3);
const auto velocity = preintegrated.segment<3>(6);
// This functor allows for saving computation when exponential map and its
// derivatives are needed at the same location in so<3>
so3::DexpFunctor local(theta);
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent * dt, // theta
position + velocity * dt + a_nav * dt22, // position
velocity + a_nav * dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;
C->block<3, 3>(3, 0) = Z_3x3;
C->block<3, 3>(6, 0) = Z_3x3;
}
return preintegratedPlus;
}
分析
首先可以看到,预积分量的代码表达 为一个九维向量: p r e i n t e g r a t e d = [ θ , x , v ] preintegrated=[\theta,x,v] preintegrated=[θ,x,v]。
接着,这里实例化了一套局部坐标系,关于这个local,gtsam的文档有详细介绍。参见:a new imu factor
然后是一个重要操作,借助局部坐标把角速度的切向量求取回来。
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
切向量求取
让我们跳转进去,看看这个函数的实现
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp* D_dexpv_omega;
}
if (H2) *H2 = invDexp;
return c;
}
代码里的变量对应论文的公式表达
dexp_
: H ( θ ) H(\theta ) H(θ),这个量是在初始化局部坐标系的时候就求好了的invDexp
: H ( θ ) − 1 H(\theta )^{-1} H(θ)−1, 这个量就是借助dexp_
这个矩阵用Eigen直接矩阵求逆得到的v
: ω k b \omega_k^b ωkbD_dexpv_omega
:用来更新噪声传播- 总的来看这个函数返回值赋值给切向量可以表达为 w_tangent = − H ( θ ) − 1 ω k b \text{w\_tangent}=-H(\theta )^{-1}\omega_k^b w_tangent=−H(θ)−1ωkb
这里特别感兴趣 H ( θ ) H(\theta ) H(θ)这个量的实现,找到local坐标系里面对其的求取:
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero)
dexp_ = I_3x3 - 0.5 * W;
else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
}
}
W
: [ ω ] × [\omega]_{\times} [ω]×theta
: ∣ ∣ ω ∣ ∣ ||\omega|| ∣∣ω∣∣one_minus_cos
: 1 − c o s ∣ ∣ ω ∣ ∣ 1-cos||\omega|| 1−cos∣∣ω∣∣K
: [ w ] × ∣ ∣ ω ∣ ∣ \frac{[w]_\times}{||\omega||} ∣∣ω∣∣[w]×KK
: [ w ] × 2 ∥ ∣ ω ∣ ∣ 2 {\frac{[w]_\times^2}{\||\omega||^2}} ∥∣ω∣∣2[w]×2
分析可以看到其计算方式为
H
(
ω
)
=
{
I
−
1
2
[
ω
]
×
∣
∣
ω
∣
∣
≈
0
I
−
(
1
−
cos
∣
∣
ω
∣
∣
)
[
ω
]
×
∣
∣
ω
∣
∣
2
+
(
1
−
sin
∣
∣
ω
∣
∣
∣
∣
ω
∣
∣
)
(
[
ω
]
×
∣
∣
ω
∣
∣
)
2
others
H(\omega)= \begin{cases} I-\frac{1}{2}[\omega]_\times& ||\omega|| \approx 0\\ I-\frac{\left(1-\cos ||\omega||)\right[\omega]_\times}{||\omega||^2}+\left(1-\frac{\sin ||\omega||}{||\omega||}\right)\left(\frac{[\omega]_\times}{||\omega||}\right)^2& \text{others} \end{cases}
H(ω)=⎩⎨⎧I−21[ω]×I−∣∣ω∣∣2(1−cos∣∣ω∣∣)[ω]×+(1−∣∣ω∣∣sin∣∣ω∣∣)(∣∣ω∣∣[ω]×)2∣∣ω∣∣≈0others
本质就是以右扰动定义的雅克比,参见下文献中的公式10.86的 J r ( x ) J_r(x) Jr(x),此外注意这里的 ω \omega ω是初始化local的时候传入的 θ \theta θ,不是测量的角速度 ω \omega ω
Gregory S. Chirikjian. “Stochastic Models, Information Theory, and Lie Groups, Volume 2” (2012).
更新预积分的测量值
preintegratedPlus << theta + w_tangent * dt, position + velocity * dt + a_nav * dt22, velocity + a_nav * dt;
θ k + 1 = θ k + H ( θ k ) − 1 ω k b Δ t p k + 1 = p k + v k Δ t + R k a k b Δ t 2 2 v k + 1 = v k + R k a k b Δ t \begin{aligned} \theta_{k+1} &=\theta_{k}+H\left(\theta_{k}\right)^{-1} \omega_{k}^{b} \Delta_{t} \\ p_{k+1} &=p_{k}+v_{k} \Delta_{t}+R_{k} a_{k}^{b} \frac{\Delta_{t}^{2}}{2} \\ v_{k+1} &=v_{k}+R_{k} a_{k}^{b} \Delta_{t} \end{aligned} θk+1pk+1vk+1=θk+H(θk)−1ωkbΔt=pk+vkΔt+Rkakb2Δt2=vk+RkakbΔt
最后,针对误差传递 Σ k + 1 = A k Σ k A k T + B k Σ η a d B k + C k Σ η g d C k \Sigma_{k+1}=A_{k} \Sigma_{k} A_{k}^{T}+B_{k}\Sigma_{\eta}^{a d} B_{k}+C_{k} \Sigma_{\eta}^{g d} C_{k} Σk+1=AkΣkAkT+BkΣηadBk+CkΣηgdCk,更新A,B,C
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;
C->block<3, 3>(3, 0) = Z_3x3;
C->block<3, 3>(6, 0) = Z_3x3;
对应公式
A
k
≈
[
I
3
×
3
−
Δ
t
2
[
ω
k
b
]
×
0
0
R
k
[
−
a
k
b
]
×
H
(
θ
k
)
Δ
t
2
I
3
×
3
I
3
×
3
Δ
t
R
k
[
−
a
k
b
]
×
H
(
θ
k
)
Δ
t
0
I
3
×
3
]
A_{k} \approx\left[\begin{array}{ccc} I_{3 \times 3}-\frac{\Delta_{t}}{2}\left[\omega_{k}^{b}\right]_{\times} &0 & 0\\ R_{k}\left[-a_{k}^{b}\right]_{\times} H\left(\theta_{k}\right) \frac{\Delta_{t}}{2} & I_{3 \times 3} & I_{3 \times 3} \Delta_{t} \\ R_{k}\left[-a_{k}^{b}\right]_{\times} H\left(\theta_{k}\right) \Delta_{t} &0 & I_{3 \times 3} \end{array}\right]
Ak≈⎣⎢⎡I3×3−2Δt[ωkb]×Rk[−akb]×H(θk)2ΔtRk[−akb]×H(θk)Δt0I3×300I3×3ΔtI3×3⎦⎥⎤
B k = [ 0 3 × 3 R k Δ t 2 R k Δ t ] , C k = [ H ( θ k ) − 1 Δ t 0 3 × 3 0 3 × 3 ] B_{k}=\left[\begin{array}{c} 0_{3 \times 3} \\ R_{k} \frac{\Delta_{t}}{2} \\ R_{k} \Delta_{t} \end{array}\right], \quad C_{k}=\left[\begin{array}{c} H\left(\theta_{k}\right)^{-1} \Delta_{t} \\ 0_{3 \times 3} \\ 0_{3 \times 3} \end{array}\right] Bk=⎣⎡03×3Rk2ΔtRkΔt⎦⎤,Ck=⎣⎡H(θk)−1Δt03×303×3⎦⎤
回到main函数
当读取到GPS的数据的时候,首先将两帧GPS数据之间的IMU预积分量添加到图优化的框架里面去。
PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1),
*preint_imu);
graph->add(imu_factor);
此外,针对两帧GPS数据之间的IMU bias的变化量添加一个约束。
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
B(correction_count ),
zero_bias, bias_noise_model));
添加gps测量约束,然后对图进行优化。