diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h index f579ae2dc44fc2f7cd5a6d1b70ba49df21dfdcb7..316405e50afa3ce2a620766e6c3a910615797536 100644 --- a/include/imu/math/imu2d_tools.h +++ b/include/imu/math/imu2d_tools.h @@ -257,7 +257,7 @@ inline void compose(const MatrixBase<D1>& d1, // Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR // Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR // -// // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!! +// // Jac wrt first delta // J_sum_d1.clear(); // J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I // J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h index 61cc6b236b0836f6ad473c5239f9a7198354aa02..c5744fbae31fea609aad91e70da9cf55cf83bb66 100644 --- a/include/imu/math/imu_tools.h +++ b/include/imu/math/imu_tools.h @@ -282,7 +282,7 @@ inline void compose(const VectorComposite& d1, Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR - // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!! + // Jac wrt first delta J_sum_d1.clear(); J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo