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=t1t0
Δba=ba0bgr
Δbg=bg0bgr
Δ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Δt0.5gWΔtΔt
Wδv=v0v1gWΔ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 中赋值: dpdbgdαdbgCC
F0(0:2,0:2)=CSW0
F0(0:2,3:5)=CSW0[Wδp]×
F0(0:2,6:8)=CSW0I(3,3)Δt
F0(0:2,9:11)=dpdbg
F0(0:2,12:14)=C
F0(3:5,3:5)=(qL{Dqq{TWS1}1}qR{TWS0})(0:2,0:2
F0(3:5,9:11)=(qR{q{TWS1}1q{TWS0}}qR{Dq})(0:2,0:2)dαdbg
F0(6:8,3:5)=CSW0Wδv
F0(6:8,6:8)=CSW0
F0(6:8,9:11)=dvdbg
F0(6:8,12:14)=C

F0=CSW00000CSW0[Wδp]×(qL{Dqq{TWS1}1}qR{TWS0})(0:2,0:2CSW0Wδv00CSW0I(3,3)Δt0CSW000dpdbg(qR{q{TWS1}1q{TWS0}}qR{Dq})(0:2,0:2)dαdbgdvdbg00C0C00

// 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)0000CSW00000I(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)=CSW0Wδp+a+F0(0:2,9:14)Δb
e(3:5)=2(Dq(q{TWS1}1q{TWS0}))v
e(6:8)=CSW0Wδv+a+F0(6:8,9:14)Δb
e(9:14)=b0b1

e(0,15)={CSW0Wδp+a+F0(0:2,9:14)Δb2(Dq(q{TWS1}1q{TWS0}))vCSW0Wδv+a+F0(6:8,9:14)Δbb0b1}

// error weighting
Eigen::Mapdouble, 15, 1> > weighted_error(residuals);
weighted_error = squareRootInformation_ * error;

r=Σ1e

输出雅各比

需要计算对参数: 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=Σ1F0(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=J0miniJlift

  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)=Σ1F0(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=Σ1F1(0:14,0:5)
Jlift=liftJacobian(TWS1)
J2=J2miniJlift

  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=Σ1F1(0:14,6:14)

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

你可能感兴趣的:(VIO,OKVIS)