From 0cde012de1e573ba5182e69fe14b586ccec53065 Mon Sep 17 00:00:00 2001 From: Dinesh Atchuthan <datchuth@laas.fr> Date: Wed, 14 Dec 2016 14:19:12 +0100 Subject: [PATCH] correct jacobians initialization in constructors --- src/feature_imu.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp index aaba65b97..08db02f5f 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); } -- GitLab