diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp index bee20f170bca5f7dc3e968cb5dba7a535aa98e1d..aaba65b97d6fbeb63ac88e81617905eb43153c49 100644 --- a/src/feature_imu.cpp +++ b/src/feature_imu.cpp @@ -4,14 +4,14 @@ namespace wolf { FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance) : FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance), - dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.segment<3>(7)), dq_preint_(_delta_preintegrated.segment<4>(3)) + dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.tail<3>()), dq_preint_(_delta_preintegrated.segment<4>(3)) { //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl; } FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Vector3s& _acc_bias, const Eigen::Vector3s& _gyro_bias, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians) : FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance), - dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.segment<3>(7)), dq_preint_(_delta_preintegrated.segment<4>(3)), + 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)) @@ -27,7 +27,7 @@ FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance) this->setCapturePtr(_cap_imu_ptr); //TODO : we should make sure here that there is a frame in the capture dp_preint_ = _delta_preintegrated.head<3>(); - dv_preint_ = _delta_preintegrated.segment<3>(7); + dv_preint_ = _delta_preintegrated.tail<3>(); dq_preint_ = _delta_preintegrated.segment<4>(3); acc_bias_preint_ = std::static_pointer_cast<wolf::FrameIMU>(_cap_imu_ptr->getFramePtr())->getAccBiasPtr()->getVector();