diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp index aaba65b97d6fbeb63ac88e81617905eb43153c49..08db02f5ffa162e59454e3bac6b9ed92f24c6c63 100644 --- a/src/feature_imu.cpp +++ b/src/feature_imu.cpp @@ -13,8 +13,8 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen: FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance), dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.tail<3>()), dq_preint_(_delta_preintegrated.segment<4>(3)), acc_bias_preint_(_acc_bias), gyro_bias_preint_(_gyro_bias), - dDp_dab_(_dD_db_jacobians.block(3,3,0,0)), dDv_dab_(_dD_db_jacobians.block(3,3,6,0)), - dDp_dwb_(_dD_db_jacobians.block(3,3,0,3)), dDv_dwb_(_dD_db_jacobians.block(3,3,6,3)), dDq_dwb_(_dD_db_jacobians.block(3,3,3,3)) + dDp_dab_(_dD_db_jacobians.block(0,0,3,3)), dDv_dab_(_dD_db_jacobians.block(6,0,3,3)), + dDp_dwb_(_dD_db_jacobians.block(0,3,3,3)), dDv_dwb_(_dD_db_jacobians.block(6,3,3,3)), dDq_dwb_(_dD_db_jacobians.block(3,3,3,3)) { //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl; } @@ -33,10 +33,10 @@ FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance) acc_bias_preint_ = std::static_pointer_cast<wolf::FrameIMU>(_cap_imu_ptr->getFramePtr())->getAccBiasPtr()->getVector(); gyro_bias_preint_ = std::static_pointer_cast<wolf::FrameIMU>(_cap_imu_ptr->getFramePtr())->getGyroBiasPtr()->getVector(); - dDp_dab_ = _dD_db_jacobians.block(3,3,0,0); - dDv_dab_ = _dD_db_jacobians.block(3,3,6,0); - dDp_dwb_ = _dD_db_jacobians.block(3,3,0,3); - dDv_dwb_ = _dD_db_jacobians.block(3,3,6,3); + dDp_dab_ = _dD_db_jacobians.block(0,0,3,3); + dDv_dab_ = _dD_db_jacobians.block(6,0,3,3); + dDp_dwb_ = _dD_db_jacobians.block(0,3,3,3); + dDv_dwb_ = _dD_db_jacobians.block(6,3,3,3); dDq_dwb_ = _dD_db_jacobians.block(3,3,3,3); }