From dd44fdab74ad6229b1200036ccf806ab784fb771 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 24 Jun 2020 23:08:45 +0200 Subject: [PATCH] Fix test. --- test/src/test_spline_opt.cpp | 8 +++++--- thirdparty/basalt-headers | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/test/src/test_spline_opt.cpp b/test/src/test_spline_opt.cpp index 6384e10..d0f97e1 100644 --- a/test/src/test_spline_opt.cpp +++ b/test/src/test_spline_opt.cpp @@ -52,6 +52,7 @@ TEST(SplineOpt, SplineOptTest) { spline_opt.resetCalib(0, {}); spline_opt.initSpline(gt_spline); + spline_opt.setG(g + Eigen::Vector3d::Random() / 10); spline_opt.init(); double error; @@ -84,8 +85,8 @@ TEST(SplineOpt, SplineOptTest) { Eigen::Vector3d pos_gt = pose_gt.translation(); Eigen::Vector3d pos = pose.translation(); - Eigen::Vector4d quat_gt = pose_gt.unit_quaternion().coeffs(); - Eigen::Vector4d quat = pose.unit_quaternion().coeffs(); + Eigen::Quaterniond quat_gt = pose_gt.unit_quaternion(); + Eigen::Quaterniond quat = pose.unit_quaternion(); Eigen::Vector3d accel_gt = gt_spline.transAccelWorld(t_ns); Eigen::Vector3d accel = spline_opt.getSpline().transAccelWorld(t_ns); @@ -95,7 +96,8 @@ TEST(SplineOpt, SplineOptTest) { ASSERT_TRUE(pos_gt.isApprox(pos)) << "pos_gt and pos are not the same"; - ASSERT_TRUE(quat_gt.isApprox(quat)) << "quat_gt and quat are not the same"; + ASSERT_TRUE(quat_gt.angularDistance(quat) < 1e-2) + << "quat_gt and quat are not the same"; ASSERT_TRUE(accel_gt.isApprox(accel)) << "accel_gt and accel are not the same"; diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index d6a1bc2..56357bb 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit d6a1bc24376a67b11bd13e44cf75c1e4351f359a +Subproject commit 56357bb4d347bedc37a47dae7d0294194b97a11a