From adfbc87a2a355a9fb5cd0acb001cc3d945062cba Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Mon, 6 Nov 2017 20:20:15 +0100
Subject: [PATCH] Cosmetics
---
src/test/gtest_imu_tools.cpp | 26 +++++++++++++-------------
1 file changed, 13 insertions(+), 13 deletions(-)
diff --git a/src/test/gtest_imu_tools.cpp b/src/test/gtest_imu_tools.cpp
index af2fed722..e53214a6b 100644
--- a/src/test/gtest_imu_tools.cpp
+++ b/src/test/gtest_imu_tools.cpp
@@ -462,7 +462,7 @@ TEST(IMU_tools, delta_correction)
ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
}
-TEST(IMU_tools, full_delta)
+TEST(IMU_tools, full_delta_residual)
{
VectorXs x1(10), x2(10);
VectorXs Delta(10), Delta_preint(10), Delta_corr(10);
@@ -475,9 +475,9 @@ TEST(IMU_tools, full_delta)
Quaternions q0; q0.setIdentity();
x1 << 0,0,0, 0,0,0,1, 0,0,0;
-// motion << .3, .2, .1, .1, .2, .3; // acc and gyro
+ motion << .3, .2, .1, .1, .2, .3; // acc and gyro
// motion << .3, .2, .1, .0, .0, .0; // only acc
- motion << .0, .0, .0, .1, .2, .3; // only gyro
+// motion << .0, .0, .0, .1, .2, .3; // only gyro
bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
bias_null << 0, 0, 0, 0, 0, 0;
// bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003;
@@ -487,30 +487,23 @@ TEST(IMU_tools, full_delta)
// preintegration with no bias
Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt);
- WOLF_TRACE("Delta real: ", Delta_real.transpose());
// final state
x2 = composeOverState(x1, Delta_real, 1000*dt);
- WOLF_TRACE("x2: ", x2.transpose());
// preintegration with the correct bias
Delta = integrateDelta(N, q0, motion, bias, bias, dt);
- WOLF_TRACE("Delta: ", Delta.transpose());
ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4);
// preintegration with wrong bias
Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
- WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
- WOLF_TRACE("Jac bias: \n", J_b);
// compute correction step
Vector9s step = J_b*(bias-bias_preint);
- WOLF_TRACE("Delta step: ", step.transpose());
// correct preintegrated delta
Delta_corr = plus(Delta_preint, step);
- WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
// Corrected delta should match real delta
ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
@@ -520,20 +513,17 @@ TEST(IMU_tools, full_delta)
// expected delta
Delta_exp = betweenStates(x1, x2, N*dt);
- WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
// compute diff between expected and corrected
Matrix<Scalar, 9, 9> J_err_exp, J_err_corr;
diff(Delta_exp, Delta_corr, Delta_err, J_err_exp, J_err_corr);
- WOLF_TRACE("Delta err: ", Delta_err.transpose());
ASSERT_MATRIX_APPROX(Delta_err, Vector9s::Zero(), 1e-3);
// compute error between expected and preintegrated
Delta_err = diff(Delta_preint, Delta_exp);
- WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
// diff between preint and corrected deltas should be the jacobian-computed step
ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3);
@@ -544,6 +534,16 @@ TEST(IMU_tools, full_delta)
* residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint)
*/
+// WOLF_TRACE("Delta real: ", Delta_real.transpose());
+// WOLF_TRACE("x2: ", x2.transpose());
+// WOLF_TRACE("Delta: ", Delta.transpose());
+// WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
+// WOLF_TRACE("Jac bias: \n", J_b);
+// WOLF_TRACE("Delta step: ", step.transpose());
+// WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
+// WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
+// WOLF_TRACE("Delta err: ", Delta_err.transpose());
+// WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
}
int main(int argc, char **argv)
--
GitLab