From 492747e9bd2270e438fb5f056980c461584d9d31 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Wed, 14 Dec 2016 12:27:27 +0100
Subject: [PATCH] FeatureIMU correct dv_preint initialization
---
src/feature_imu.cpp | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp
index bee20f170..aaba65b97 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();
--
GitLab