From ab45fedf8f17ff863d027951f45940d5eb563494 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 9 Sep 2022 09:06:39 +0200
Subject: [PATCH] Remove cast<T> and some size errors
---
include/imu/factor/factor_imu.h | 18 +++++++++---------
1 file changed, 9 insertions(+), 9 deletions(-)
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 49f504335..7b37d079e 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -216,7 +216,7 @@ inline bool FactorImu::operator ()(const T* const _p1,
Map<const Quaternion<T> > q2(_q2);
Map<const Matrix<T,3,1> > v2(_v2);
- Map<Matrix<T,15,1> > res(_res);
+ Map<Matrix<T,9,1> > res(_res);
residual(p1, q1, v1, ab1, wb1, p2, q2, v2, res);
@@ -301,23 +301,23 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
//needed typedefs
typedef typename D1::Scalar T;
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 15);
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 9);
// 1. Expected delta from states
Eigen::Matrix<T, 3, 1 > dp_exp;
Eigen::Quaternion<T> dq_exp;
Eigen::Matrix<T, 3, 1> dv_exp;
- imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
+ imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, dt_, dp_exp, dq_exp, dv_exp);
// 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
// 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
Eigen::Matrix<T, 9, 1> d_step;
- d_step.block(0,0,3,1) = dDp_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDp_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(3,0,3,1) = dDq_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(6,0,3,1) = dDv_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDv_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
+ d_step.block(0,0,3,1) = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(3,0,3,1) = dDq_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(6,0,3,1) = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_);
#ifdef METHOD_1 // method 1
Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1);
@@ -351,10 +351,10 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
J_err_corr.block(3,3,3,3) = J_do_dq2;
// 4. Residuals are the weighted errors
- _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+ _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper() * d_error;
#else
imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error);
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
#else // method 2
@@ -372,7 +372,7 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
d_error << d_diff - d_step;
// 4. Residuals are the weighted errors
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
--
GitLab