OKVIS IMU 误差公式代码版本
公式来源 IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation 的 supplementary material
公式和思路和预积分论文是一致的，和OKVIS IJRR论文里说的不一样！
ceres 优化时，迭代更新状态量，需要计算IMU的error。因为预积分时需要用到状态量IMU的偏置，而状态量在迭代中是变化的，所以当偏置变化小时，根据状态量对偏置的雅克比更新偏置变化后的积分值，当偏置变化大时，再重新积分。
误差来源
// imuParametersVec_ 元素个数等于 IMU 个数，这里只有一个 IMU
// add IMU error terms
for (size_t i = 0; i < imuParametersVec_.size(); ++i) {
std::shared_ptr imuError(
new ceres::ImuError(imuMeasurements, imuParametersVec_.at(i),
lastElementIterator->second.timestamp,
states.timestamp));
/*::ceres::ResidualBlockId id = */mapPtr_->addResidualBlock(
imuError,
NULL,
mapPtr_->parameterBlockPtr(lastElementIterator->second.id),
mapPtr_->parameterBlockPtr(
lastElementIterator->second.sensors.at(SensorStates::Imu).at(i).at(
ImuSensorStates::SpeedAndBias).id),
mapPtr_->parameterBlockPtr(states.id),
mapPtr_->parameterBlockPtr(
states.sensors.at(SensorStates::Imu).at(i).at(
ImuSensorStates::SpeedAndBias).id)); // Construct with measurements and parameters.
ImuError::ImuError(const okvis::ImuMeasurementDeque & imuMeasurements,
const okvis::ImuParameters & imuParameters,
const okvis::Time& t_0, const okvis::Time& t_1) {
setImuMeasurements(imuMeasurements);
setImuParameters(imuParameters);
setT0(t_0);
setT1(t_1);
OKVIS_ASSERT_TRUE_DBG(Exception,
t_0 >= imuMeasurements.front().timeStamp,
"First IMU measurement included in ImuError is not old enough!");
OKVIS_ASSERT_TRUE_DBG(Exception,
t_1 <= imuMeasurements.back().timeStamp,
"Last IMU measurement included in ImuError is not new enough!");
}和 ceres 接口 Evaluate 函数
// This evaluates the error term and additionally computes the Jacobians.
bool ImuError::Evaluate(double const* const * parameters, double* residuals,
double** jacobians) const {
return EvaluateWithMinimalJacobians(parameters, residuals, jacobians, NULL);
}
// This evaluates the error term and additionally computes
// the Jacobians in the minimal internal representation.
bool ImuError::EvaluateWithMinimalJacobians(double const* const * parameters,
double* residuals,
double** jacobians,
double** jacobiansMinimal) const {
// get poses
const okvis::kinematics::Transformation T_WS_0(
Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]),
Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]));
const okvis::kinematics::Transformation T_WS_1(
Eigen::Vector3d(parameters[2][0], parameters[2][1], parameters[2][2]),
Eigen::Quaterniond(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]));
// get speed and bias
okvis::SpeedAndBias speedAndBiases_0;
okvis::SpeedAndBias speedAndBiases_1;
for (size_t i = 0; i < 9; ++i) {
speedAndBiases_0[i] = parameters[1][i];
speedAndBiases_1[i] = parameters[3][i];
}
// this will NOT be changed:
const Eigen::Matrix3d C_WS_0 = T_WS_0.C();
const Eigen::Matrix3d C_S0_W = C_WS_0.transpose();输入参数：
上一帧 sensor 系到世界系的转换矩阵： TWS0
当前帧 sensor 系到世界系的转换矩阵： TWS1
上一帧 sensor 系到世界系转换矩阵旋转部分： CWS0=C{TWS0}
CSW0=CTWS0
上一帧： v0bg0ba0b0=[bg0 ba0]
当前帧： v1bg1ba1b1=[bg1 ba1]
// call the propagation
const double Delta_t = (t1_ - t0_).toSec();
Eigen::Matrix<double, 6, 1> Delta_b;
// ensure unique access
{
std::lock_guard<std::mutex> lock(preintegrationMutex_);
Delta_b = speedAndBiases_0.tail<6>()
- speedAndBiases_ref_.tail<6>();
}
redo_ = redo_ || (Delta_b.head<3>().norm() * Delta_t > 0.0001);
if (redo_) {
/* 当偏置变换大时，再重新积分，预积分实际只和优化的状态量上一帧的偏置有关，
所以这里当偏置变化大的时重新计算预积分的值，当偏置变化不大时，根据雅克比更新预积分的值。*/
redoPreintegration(T_WS_0, speedAndBiases_0);
redoCounter_++;
Delta_b.setZero();
redo_ = false;
/*if (redoCounter_ > 1) {
std::cout << "pre-integration no. " << redoCounter_ << std::endl;
}*/
} Δt=t1−t0
Δba=ba0−bgr
Δbg=bg0−bgr
Δb=[Δbg; Δba]
if redo_ || ||Δbg||Δt>0.0001
redoPreintegration(T_WS_0, speedAndBiases_0);
redoCounter_++;
Delta_b.setZero();
redo_ = false;end if
actual propagation output
// actual propagation output:
{
std::lock_guard<std::mutex> lock(preintegrationMutex_); // this is a bit stupid, but shared read-locks only come in C++14
const Eigen::Vector3d g_W = imuParameters_.g * Eigen::Vector3d(0, 0, 6371009).normalized();
// assign Jacobian w.r.t. x0
Eigen::Matrix<double,15,15> F0 =
Eigen::Matrix<double,15,15>::Identity(); // holds for d/db_g, d/db_a
const Eigen::Vector3d delta_p_est_W =
T_WS_0.r() - T_WS_1.r() + speedAndBiases_0.head<3>()*Delta_t - 0.5*g_W*Delta_t*Delta_t;
const Eigen::Vector3d delta_v_est_W =
speedAndBiases_0.head<3>() - speedAndBiases_1.head<3>() - g_W*Delta_t;
const Eigen::Quaterniond Dq = okvis::kinematics::deltaQ(-dalpha_db_g_*Delta_b.head<3>())*Delta_q_;读入的配置参数，重力加速度： g
Δ′qdαdbg′ 是在 redoPreintegration 中计算的，也就是说状态量对偏置的雅克比是在 redoPreintegration 中计算的，预积分时需要用到状态量对偏置的雅可比，只有当偏置变化大时，才做 redoPreintegration，重新计算导数，思路和预积分论文是一致的!
gW=g∗(0,0,1)T
F0=I(15,15)
Wδp=t{TWS0}−t{TWS1}+v0∗Δt−0.5∗gW∗Δt∗Δt
Wδv=v0−v1−gW∗Δt
Dq=δQ{−dαdbg′∗Δbg}∗Δ′q
F0.block<3,3>(0,0) = C_S0_W;
F0.block<3,3>(0,3) = C_S0_W * okvis::kinematics::crossMx(delta_p_est_W);
F0.block<3,3>(0,6) = C_S0_W * Eigen::Matrix3d::Identity()*Delta_t;
F0.block<3,3>(0,9) = dp_db_g_;
F0.block<3,3>(0,12) = -C_doubleintegral_;
F0.block<3,3>(3,3) = (okvis::kinematics::plus(Dq*T_WS_1.q().inverse()) *
okvis::kinematics::oplus(T_WS_0.q())).topLeftCorner<3,3>();
F0.block<3,3>(3,9) = (okvis::kinematics::oplus(T_WS_1.q().inverse()*T_WS_0.q())*
okvis::kinematics::oplus(Dq)).topLeftCorner<3,3>()*(-dalpha_db_g_);
F0.block<3,3>(6,3) = C_S0_W * okvis::kinematics::crossMx(delta_v_est_W);
F0.block<3,3>(6,6) = C_S0_W;
F0.block<3,3>(6,9) = dv_db_g_;
F0.block<3,3>(6,12) = -C_integral_;F0 是对状态量 x0 的雅克比，推导见 IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation 的 supplementary material
redoPreintegration 中赋值: dpdbg′dαdbg′∫C∬C
F0(0:2,0:2)=CSW0
F0(0:2,3:5)=CSW0∗[Wδp]×
F0(0:2,6:8)=CSW0∗I(3,3)∗Δt
F0(0:2,9:11)=dpdbg′
F0(0:2,12:14)=−∬C
F0(3:5,3:5)=(qL{Dq∗q{TWS1}−1}∗qR{TWS0})(0:2,0:2
F0(3:5,9:11)=(qR{q{TWS1}−1∗q{TWS0}}∗qR{Dq})(0:2,0:2)∗−dαdbg′
F0(6:8,3:5)=CSW0∗Wδv
F0(6:8,6:8)=CSW0
F0(6:8,9:11)=dvdbg
F0(6:8,12:14)=−∫C
F0=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪CSW00000CSW0∗[Wδp]×(qL{Dq∗q{TWS1}−1}∗qR{TWS0})(0:2,0:2CSW0∗Wδv00CSW0∗I(3,3)∗Δt0CSW000dpdbg′(qR{q{TWS1}−1∗q{TWS0}}∗qR{Dq})(0:2,0:2)∗−dαdbg′dvdbg00−∬C0−∫C00⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪
// assign Jacobian w.r.t. x1
Eigen::Matrix<double,15,15> F1 =
-Eigen::Matrix<double,15,15>::Identity(); // holds for the biases
F1.block<3,3>(0,0) = -C_S0_W;
F1.block<3,3>(3,3) = -(okvis::kinematics::plus(Dq) *
okvis::kinematics::oplus(T_WS_0.q()) *
okvis::kinematics::plus(T_WS_1.q().inverse())).topLeftCorner<3,3>();
F1.block<3,3>(6,6) = -C_S0_W; F1 是对状态量 x1 的雅克比，推导见 IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation 的 supplementary material
F1=−I(15,15)
F1(0:2,0:2)=−CSW0
F1(3:5,3:5)=−(qL{Dq}∗qR{q{TWS0}}∗qL{q{TWS1}−1})(0:2,0:2)
F1(6:8,6:8)=−CSW0
F1=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪−CSW00000−(qL{Dq}∗qR{q{TWS0}}∗qL{q{TWS1}−1})(0:2,0:2)0000−CSW00000−I(6,6)⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪
输出的 error
// the overall error vector
Eigen::Matrix<double, 15, 1> error;
error.segment<3>(0) = C_S0_W * delta_p_est_W + acc_doubleintegral_ + F0.block<3,6>(0,9)*Delta_b;
error.segment<3>(3) = 2*(Dq*(T_WS_1.q().inverse()*T_WS_0.q())).vec(); //2*T_WS_0.q()*Dq*T_WS_1.q().inverse();//
error.segment<3>(6) = C_S0_W * delta_v_est_W + acc_integral_ + F0.block<3,6>(6,9)*Delta_b;
error.tail<6>() = speedAndBiases_0.tail<6>() - speedAndBiases_1.tail<6>(); e(0:2)=CSW0∗Wδp+∬a+F0(0:2,9:14)∗Δb
e(3:5)=2∗(Dq∗(q{TWS1}−1∗q{TWS0}))v
e(6:8)=CSW0∗Wδv+∫a+F0(6:8,9:14)∗Δb
e(9:14)=b0−b1
e(0,15)={CSW0∗Wδp+∬a+F0(0:2,9:14)∗Δb2∗(Dq∗(q{TWS1}−1∗q{TWS0}))vCSW0∗Wδv+∫a+F0(6:8,9:14)∗Δbb0−b1}
// error weighting
Eigen::Mapdouble, 15, 1> > weighted_error(residuals);
weighted_error = squareRootInformation_ * error; r=Σ−1−−−√∗e
输出雅各比
需要计算对参数： TWS0speedAndBiases0TWS0speedAndBiases1
雅各比： J0J1J2J3
if (jacobians != NULL) {
if (jacobians[0] != NULL) {
// Jacobian w.r.t. minimal perturbance
Eigen::Matrix<double, 15, 6> J0_minimal = squareRootInformation_
* F0.block<15, 6>(0, 0);
// pseudo inverse of the local parametrization Jacobian:
Eigen::Matrix<double, 6, 7, Eigen::RowMajor> J_lift;
PoseLocalParameterization::liftJacobian(parameters[0], J_lift.data());redoPreintegration 中赋值： Σ−1−−−√
J0mini=Σ−1−−−√∗F0(0:14,0:5)
Jlift=liftJacobian(TWS0)
// hallucinate Jacobian w.r.t. state
Eigen::Mapdouble, 15, 7, Eigen::RowMajor> > J0(
jacobians[0]);
J0 = J0_minimal * J_lift;
// if requested, provide minimal Jacobians
if (jacobiansMinimal != NULL) {
if (jacobiansMinimal[0] != NULL) {
Eigen::Mapdouble, 15, 6, Eigen::RowMajor> > J0_minimal_mapped(
jacobiansMinimal[0]);
J0_minimal_mapped = J0_minimal;
}
}
} J0=J0mini∗Jlift
if (jacobians[1] != NULL) {
Eigen::Mapdouble, 15, 9, Eigen::RowMajor> > J1(
jacobians[1]);
J1 = squareRootInformation_ * F0.block<15, 9>(0, 6);
// if requested, provide minimal Jacobians
if (jacobiansMinimal != NULL) {
if (jacobiansMinimal[1] != NULL) {
Eigen::Mapdouble, 15, 9, Eigen::RowMajor> > J1_minimal_mapped(
jacobiansMinimal[1]);
J1_minimal_mapped = J1;
}
}
} J1(15,9)=Σ−1−−−√∗F0(0:14,6:14)
if (jacobians[2] != NULL) {
// Jacobian w.r.t. minimal perturbance
Eigen::Matrix<double, 15, 6> J2_minimal = squareRootInformation_
* F1.block<15, 6>(0, 0);
// pseudo inverse of the local parametrization Jacobian:
Eigen::Matrix<double, 6, 7, Eigen::RowMajor> J_lift;
PoseLocalParameterization::liftJacobian(parameters[2], J_lift.data());
// hallucinate Jacobian w.r.t. state
Eigen::Mapdouble, 15, 7, Eigen::RowMajor> > J2(
jacobians[2]);
J2 = J2_minimal * J_lift;
// if requested, provide minimal Jacobians
if (jacobiansMinimal != NULL) {
if (jacobiansMinimal[2] != NULL) {
Eigen::Mapdouble, 15, 6, Eigen::RowMajor> > J2_minimal_mapped(
jacobiansMinimal[2]);
J2_minimal_mapped = J2_minimal;
}
}
} J2mini=Σ−1−−−√∗F1(0:14,0:5)
Jlift=liftJacobian(TWS1)
J2=J2mini∗Jlift
if (jacobians[3] != NULL) {
Eigen::Mapdouble, 15, 9, Eigen::RowMajor> > J3(jacobians[3]);
J3 = squareRootInformation_ * F1.block<15, 9>(0, 6);
// if requested, provide minimal Jacobians
if (jacobiansMinimal != NULL) {
if (jacobiansMinimal[3] != NULL) {
Eigen::Mapdouble, 15, 9, Eigen::RowMajor> > J3_minimal_mapped(
jacobiansMinimal[3]);
J3_minimal_mapped = J3;
}
}
} J3mini=Σ−1−−−√∗F1(0:14,6:14)