From 8d4da9630bc3e417371f9593a45fab0310ec14c8 Mon Sep 17 00:00:00 2001 From: f-fl0 Date: Wed, 9 Oct 2024 11:56:20 +0900 Subject: [PATCH] Generate mcl_3dl::State6DOF using both mcl_3dl::Vec3 and mcl_3dl::Quat --- test/src/test_state_6dof.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/test/src/test_state_6dof.cpp b/test/src/test_state_6dof.cpp index 26f2ca8..d9b44c7 100644 --- a/test/src/test_state_6dof.cpp +++ b/test/src/test_state_6dof.cpp @@ -126,12 +126,24 @@ TEST(State6DOF, CovarianceMatrix) std::normal_distribution z_dis(e.pos_.z_, 0.3); std::normal_distribution yaw_dis(e.rot_.getRPY().z_, 0.4); + const auto generate_state = [&mt, &x_dis, &y_dis, &z_dis, &yaw_dis](const bool use_rpy) -> mcl_3dl::State6DOF + { + if (use_rpy) + { + return mcl_3dl::State6DOF(mcl_3dl::Vec3(x_dis(mt), y_dis(mt), z_dis(mt)), + mcl_3dl::Vec3(0, 0, yaw_dis(mt))); + } + else + { + return mcl_3dl::State6DOF(mcl_3dl::Vec3(x_dis(mt), y_dis(mt), z_dis(mt)), + mcl_3dl::Quat(mcl_3dl::Vec3(0.0, 0.0, 1.0), yaw_dis(mt))); + } + }; + const std::size_t N = 500; for (std::size_t n = 0; n < N; n++) { - mcl_3dl::State6DOF s( - mcl_3dl::Vec3(x_dis(mt), y_dis(mt), z_dis(mt)), - mcl_3dl::Quat(mcl_3dl::Vec3(0.0, 0.0, 1.0), yaw_dis(mt))); + mcl_3dl::State6DOF s = generate_state(n % 2 == 0); for (std::size_t j = 0; j < 6; j++) { for (std::size_t k = j; k < 6; k++)