Skip to content
Snippets Groups Projects
Commit c9ea1b48 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix all. Print full optimized trajectory

parent ed4f2dec
No related branches found
No related tags found
1 merge request!158Imu trajectory
Pipeline #
This commit is part of merge request !158. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
Showing
with 74 additions and 36 deletions
src/test/gtest_IMU.cpp
+ 74
− 36
View file @ c9ea1b48
...@@ -68,6 +68,9 @@ class Process_Constraint_IMU : public testing::Test ...@@ -68,6 +68,9 @@ class Process_Constraint_IMU : public testing::Test
VectorXs D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias VectorXs D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias
VectorXs x0_optim; // optimized using constraint_imu VectorXs x0_optim; // optimized using constraint_imu
// Trajectory buffer of correction Jacobians
std::vector<MatrixXs> Buf_Jac_preint_prc;
// Trajectory matrices // Trajectory matrices
MatrixXs Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc; MatrixXs Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
MatrixXs Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc; MatrixXs Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
...@@ -81,7 +84,6 @@ class Process_Constraint_IMU : public testing::Test ...@@ -81,7 +84,6 @@ class Process_Constraint_IMU : public testing::Test
bool p1_fixed, q1_fixed, v1_fixed; bool p1_fixed, q1_fixed, v1_fixed;
virtual void SetUp( ) virtual void SetUp( )
{ {
string wolf_root = _WOLF_ROOT_DIR; string wolf_root = _WOLF_ROOT_DIR;
...@@ -246,7 +248,7 @@ class Process_Constraint_IMU : public testing::Test ...@@ -246,7 +248,7 @@ class Process_Constraint_IMU : public testing::Test
J_D_b .setZero(); J_D_b .setZero();
q = q0; q = q0;
TimeStamp t(0); TimeStamp t(0);
// trajectory.get().push_back(processor_imu->motionZero(t)); trajectory.get().emplace_back(t, Vector6s::Zero(), M6, VectorXs::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXs::Zero(9,6));
for (int n = 0; n < motion.cols(); n++) for (int n = 0; n < motion.cols(); n++)
{ {
t += dt; t += dt;
...@@ -365,7 +367,7 @@ class Process_Constraint_IMU : public testing::Test ...@@ -365,7 +367,7 @@ class Process_Constraint_IMU : public testing::Test
// Integrate Trajectories all methods // Integrate Trajectories all methods
virtual void integrateAllTrajectories() virtual void integrateAllTrajectories()
{ {
Size cols = motion.cols(); Size cols = motion.cols() + 1;
Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols); Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols); Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
...@@ -375,10 +377,12 @@ class Process_Constraint_IMU : public testing::Test ...@@ -375,10 +377,12 @@ class Process_Constraint_IMU : public testing::Test
// Build exact trajectories // Build exact trajectories
int col = 0; int col = 0;
Scalar Dt = 0;
for (auto m : Buf_exact.get()) for (auto m : Buf_exact.get())
{ {
Trj_D_exact.col(col) = m.delta_integr_; Trj_D_exact.col(col) = m.delta_integr_;
Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, DT ); Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, Dt );
Dt += dt;
col ++; col ++;
} }
...@@ -392,17 +396,18 @@ class Process_Constraint_IMU : public testing::Test ...@@ -392,17 +396,18 @@ class Process_Constraint_IMU : public testing::Test
// Build trajectories preintegrated and corrected with imu_tools // Build trajectories preintegrated and corrected with imu_tools
col = 0; col = 0;
Dt = 0;
for (auto m : Buf_preint_imu.get()) for (auto m : Buf_preint_imu.get())
{ {
// preint // preint
Trj_D_preint_imu.col(col) = m.delta_integr_; Trj_D_preint_imu.col(col) = m.delta_integr_;
Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col), DT ); Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt );
// corrected // corrected
MatrixXs J_D_bias = m.jacobian_calib_; VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
VectorXs step = J_D_bias * (bias_real - bias_preint);
Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ; Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ;
Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col), DT ); Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt );
Dt += dt;
col ++; col ++;
} }
...@@ -422,21 +427,22 @@ class Process_Constraint_IMU : public testing::Test ...@@ -422,21 +427,22 @@ class Process_Constraint_IMU : public testing::Test
// Build trajectories preintegrated and corrected with processorIMU // Build trajectories preintegrated and corrected with processorIMU
col = -1; Dt = 0;
col = 0;
Buf_Jac_preint_prc.clear();
for (auto m : Buf_D_preint_prc.get()) for (auto m : Buf_D_preint_prc.get())
{ {
if (col != -1) // preint
{ Trj_D_preint_prc.col(col) = m.delta_integr_;
// preint Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt );
Trj_D_preint_prc.col(col) = m.delta_integr_;
Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col), DT ); // corrected
VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
// corrected Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
MatrixXs J_D_bias = m.jacobian_calib_; Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt );
VectorXs step = J_D_bias * (bias_real - bias_preint);
Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ; Buf_Jac_preint_prc.push_back(m.jacobian_calib_);
Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col), DT ); Dt += dt;
}
col ++; col ++;
} }
...@@ -655,6 +661,24 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU ...@@ -655,6 +661,24 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
sensor_odo->process(capture_odo); sensor_odo->process(capture_odo);
} }
virtual void integrateAllTrajectories() override
{
// ===================================== IMU
Process_Constraint_IMU::integrateAllTrajectories();
// ===================================== ODO
Vector6s data;
Vector3s p1 = x1_exact.head(3);
Quaternions q1 (x1_exact.data() + 3);
Vector3s dp = q0.conjugate() * (p1 - p0);
Vector3s dth = log_q( q0.conjugate() * q1 );
data << dp, dth;
CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
sensor_odo->process(capture_odo);
}
virtual void buildProblem() override virtual void buildProblem() override
{ {
// ===================================== IMU // ===================================== IMU
...@@ -1421,9 +1445,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim ...@@ -1421,9 +1445,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
a = Matrix<Scalar, 3, 50>::Random(); a = Matrix<Scalar, 3, 50>::Random();
w = Matrix<Scalar, 3, 50>::Random(); w = Matrix<Scalar, 3, 50>::Random();
a = Matrix<Scalar,3,50>::Identity();
w = Matrix<Scalar,3,50>::Identity();
// ---------- fix boundaries // ---------- fix boundaries
p0_fixed = true; p0_fixed = true;
q0_fixed = false; q0_fixed = false;
...@@ -1439,7 +1460,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim ...@@ -1439,7 +1460,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
// ===================================== RUN ALL // ===================================== RUN ALL
string report = runAll(1); string report = runAll(1);
printAll(report); // printAll(report);
assertAll(); assertAll();
...@@ -1466,11 +1487,8 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) ...@@ -1466,11 +1487,8 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
bias_preint = -bias_real; bias_preint = -bias_real;
// ---------- motion params // ---------- motion params
a = Matrix<Scalar, 3, 50>::Random(); a = Matrix<Scalar, 3, 50>::Ones() + 0.1 * Matrix<Scalar, 3, 50>::Random();
w = Matrix<Scalar, 3, 50>::Random(); w = Matrix<Scalar, 3, 50>::Ones() + 0.1 * Matrix<Scalar, 3, 50>::Random();
a = Matrix<Scalar,3,50>::Identity();
w = Matrix<Scalar,3,50>::Identity();
// ---------- fix boundaries // ---------- fix boundaries
p0_fixed = true; p0_fixed = true;
...@@ -1486,23 +1504,43 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) ...@@ -1486,23 +1504,43 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
// ===================================== RUN ALL // ===================================== RUN ALL
configureAll(); configureAll();
// integrateAllTrajectories(); integrateAllTrajectories();
integrateAll();
buildProblem(); buildProblem();
string report = solveProblem(1); string report = solveProblem(1);
printAll(report);
assertAll(); assertAll();
// Build optimal trajectory
MatrixXs Trj_x_optim(10,Trj_D_preint_prc.cols());
Scalar Dt = 0;
Size i = 0;
for (auto J_D_b : Buf_Jac_preint_prc)
{
VectorXs step = J_D_b * (bias_0 - bias_preint);
VectorXs D_opt = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
Trj_x_optim.col(i) = imu::composeOverState(x0_optim, D_opt, Dt);
Dt += dt;
i ++;
}
// printAll(report);
WOLF_INFO("------------------------------------");
WOLF_INFO("Exact x0 \n", x0.transpose());
WOLF_INFO("Optim x0 \n", x0_optim.transpose());
WOLF_INFO("Optim x \n", Trj_x_optim.transpose());
WOLF_INFO("Optim x1 \n", x1_optim.transpose());
WOLF_INFO("Exact x1 \n", x1_exact.transpose());
WOLF_INFO("------------------------------------");
} }
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
// ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU.*"; // ::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.*MotionRandom_PqV_b__pqV_b"; ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment