公式来源 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<ceres::ImuError> 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::Map<Eigen::Matrix<double, 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::Map<Eigen::Matrix<double, 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::Map<Eigen::Matrix<double, 15, 6, Eigen::RowMajor> > J0_minimal_mapped( jacobiansMinimal[0]); J0_minimal_mapped = J0_minimal; } } }

J0=J0mini∗Jlift

if (jacobians[1] != NULL) { Eigen::Map<Eigen::Matrix<double, 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::Map<Eigen::Matrix<double, 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::Map<Eigen::Matrix<double, 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::Map<Eigen::Matrix<double, 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::Map<Eigen::Matrix<double, 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::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor> > J3_minimal_mapped( jacobiansMinimal[3]); J3_minimal_mapped = J3; } } }

J3mini=Σ−1−−−√∗F1(0:14,6:14)

版权声明：本文为博主原创文章，未经博主允许不得转载。