公式來源 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

F 0 = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ C S W 0 0000 C S W 0 * [W δ p] \times (q L {D q * q {T W S 1} - 1} * q R {T W S 0}) (0 : 2, 0 : 2 C S W 0 * W δ v 00 C S W 0 * I (3, 3) * Δ t 0 C S W 0 00 d p d b g' (q R {q {T W S 1} - 1 * q {T W S 0}} * q R {D q}) (0 : 2, 0 : 2) * - d α d b g' d v d b g 00 - \iint C 0 - \int C 00 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

// 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

F 1 = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ - C S W 0 000 0 - (q L {D q} * q R {q {T W S 0}} * q L {q {T W S 1} - 1}) (0 : 2, 0 : 2) 00 00 - C S W 0 0 000 - 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) = {C S W 0 * W δ p + \iint a + F 0 (0 : 2, 9 : 14) * Δ b 2 * (D q * (q {T W S 1} - 1 * q {T W S 0})) v C S W 0 * W δ v + \int a + F 0 (6 : 8, 9 : 14) * Δ b b 0 - b 1}

// 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)

OKVIS IMU 誤差公式代碼版本

誤差來源

和 ceres 接口 Evaluate 函數

if redo_ || ||Δbg||Δt>0.0001

end if

actual propagation output

輸出的 error

輸出雅各比

Android啓動過程-萬字長文(Android14)

這種嵌套字典類型的數據，我想把它讀取到df裏，如何操作？

微調真的能讓LLM學到新東西嗎:引入新知識可能讓模型產生更多的幻覺

iNeuOS工業互聯網操作系統，增加電力IEC104協議

微服務實踐k8s&dapr開發部署實驗（3）訂閱發佈

kbgressdb之數據結構V0.2

ElasticFusion 解析

ElasticFusion 中的 Randomized Ferns 重定位/迴環檢測論文和代碼解析

KinectFusion 中的 ICP 算法 GPU 代碼解讀

DBoW2 迴環檢測/重定位算法解析

PCL KinectFusion 編譯安裝教程

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結