diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp index d126a7a23f659587939472b5201f35f018375f96..7dddffacde11f45a982134d2a9ffb5097fd5c58e 100644 --- a/src/test/gtest_IMU.cpp +++ b/src/test/gtest_IMU.cpp @@ -221,6 +221,37 @@ class Process_Constraint_IMU : public testing::Test return Delta; } + /* Integrate acc and angVel motion, obtain Delta_preintegrated + * Input: + * q0: initial orientation + * motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame + * bias_real: the real bias of the IMU + * bias_preint: the bias used for Delta pre-integration + * Output: + * J_D_b: the Jacobian of the preintegrated delta wrt the bias + * return: the preintegrated delta + */ + static MotionBuffer integrateDeltaTrajectory(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b) + { + MotionBuffer trajectory(6, 10, 9, 6); + VectorXs Delta(10); + MatrixXs dummy(0,0); + Quaternions q; + + Delta = imu::identity(); + J_D_b .setZero(); + q = q0; + TimeStamp t(0); + for (int n = 0; n < motion.cols(); n++) + { + t += dt; + integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b); + trajectory.setCalibrationPreint(bias_preint); + trajectory.get().emplace_back(t, motion.col(n), dummy, dummy, dummy, Delta, dummy, dummy, dummy, J_D_b); + } + return trajectory; + } + void integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected) @@ -326,6 +357,45 @@ class Process_Constraint_IMU : public testing::Test } + // Integrate Trajectories all methods +// virtual void integrateAllTrajectories() +// { +// size_t cols = motion.cols(); +// MatrixXs Trj_D_exact(10,cols), Trj_D_preint_imu(10,cols), Trj_D_preint(10,cols), Trj_D_corrected(10,cols), Trj_D_corrected_imu(10,cols); +// MatrixXs Trj_x_exact(10,cols), Trj_x_preint_imu(10,cols), Trj_x_preint(10,cols), Trj_x_corrected(10,cols), Trj_x_corrected_imu(10,cols); +// // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all +// if (motion.cols() == 1) +// Trj_D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt); +// else +// Trj_D_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias); +// Trj_x_exact = imu::composeOverState(x0, D_exact, DT ); +// +// +// // ===================================== INTEGRATE USING IMU_TOOLS +// // pre-integrate +// if (motion.cols() == 1) +// Trj_D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias); +// else +// Trj_D_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias); +// +// // correct perturbated +// step = J_D_bias * (bias_real - bias_preint); +// Trj_D_corrected_imu = imu::plus(D_preint_imu, step); +// +// // compose states +// x1_preint_imu = imu::composeOverState(x0, D_preint_imu , DT ); +// x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT ); +// +// // ===================================== INTEGRATE USING PROCESSOR_IMU +// +// integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected); +// +// // compose states +// x1_preint = imu::composeOverState(x0, D_preint , DT ); +// x1_corrected = imu::composeOverState(x0, D_corrected , DT ); +// } + + // Set state_blocks status void setFixedBlocks() @@ -1322,11 +1392,60 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim } +TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated +{ + + // ================================================================================================================ // + // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== // + // ================================================================================================================ // + // + // ---------- time + t0 = 0; + num_integrations = 50; + + // ---------- initial pose + p0 << 0,0,0; + q0.coeffs() << 0,0,0,1; + v0 << 0,0,0; + + // ---------- bias + bias_real << .001, .002, .003, -.001, -.002, -.003; + bias_preint = -bias_real; + + // ---------- motion params + a = Matrix<Scalar, 3, 50>::Random(); + w = Matrix<Scalar, 3, 50>::Random(); + + // ---------- fix boundaries + p0_fixed = true; + q0_fixed = false; + v0_fixed = true; + p1_fixed = false; + q1_fixed = false; + v1_fixed = true; + // + // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // + // ================================================================================================================ // + + + // ===================================== RUN ALL + configureAll(); + integrateAll(); + buildProblem(); + string report = solveProblem(1); + + // printAll(report); + + assertAll(); + +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU.*"; -// ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.*"; + // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.*"; + ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b"; return RUN_ALL_TESTS(); }