1 #include <gtest/gtest.h> 2 3 #include <private/dvr/eigen.h> 4 #include <private/dvr/pose.h> 5 #include <private/dvr/test/test_macros.h> 6 7 using PoseTypes = ::testing::Types<float, double>; 8 9 template <class T> 10 class PoseTest : public ::testing::TestWithParam<T> { 11 public: 12 using FT = T; 13 using Pose_t = android::dvr::Pose<FT>; 14 using quat_t = Eigen::Quaternion<FT>; 15 using vec3_t = Eigen::Vector3<FT>; 16 using mat4_t = Eigen::AffineMatrix<FT, 4>; 17 }; 18 19 TYPED_TEST_CASE(PoseTest, PoseTypes); 20 21 // Check that the two matrix methods are inverses of each other 22 TYPED_TEST(PoseTest, SelfInverse) { 23 using quat_t = typename TestFixture::quat_t; 24 using vec3_t = typename TestFixture::vec3_t; 25 using Pose_t = typename TestFixture::Pose_t; 26 using mat4_t = typename TestFixture::mat4_t; 27 using FT = typename TestFixture::FT; 28 29 const auto tolerance = FT(0.0001); 30 31 const quat_t initial_rotation(Eigen::AngleAxis<FT>( 32 FT(M_PI / 3.0), vec3_t(FT(3.0), FT(4.0), FT(5.0)).normalized())); 33 const vec3_t initial_position = vec3_t(FT(2.0), FT(10.0), FT(-4.0)); 34 const Pose_t initial_pose(initial_rotation, initial_position); 35 36 auto result_pose = initial_pose.GetReferenceFromObjectMatrix() * 37 initial_pose.GetObjectFromReferenceMatrix(); 38 39 EXPECT_MAT4_NEAR(result_pose, mat4_t::Identity(), tolerance); 40 } 41 42 TYPED_TEST(PoseTest, TransformPoint) { 43 using quat_t = typename TestFixture::quat_t; 44 using vec3_t = typename TestFixture::vec3_t; 45 using Pose_t = typename TestFixture::Pose_t; 46 using FT = typename TestFixture::FT; 47 48 const auto tolerance = FT(0.0001); 49 50 const quat_t pose_rotation( 51 Eigen::AngleAxis<FT>(FT(M_PI / 2.0), vec3_t(FT(0.0), FT(0.0), FT(1.0)))); 52 const auto pose_position = vec3_t(FT(1.0), FT(1.0), FT(2.0)); 53 54 const Pose_t test_pose(pose_rotation, pose_position); 55 56 for (int axis = 0; axis < 3; ++axis) { 57 vec3_t start_position = vec3_t::Zero(); 58 start_position[axis] = FT(1.0); 59 const vec3_t expected_transformed = 60 (pose_rotation * start_position) + pose_position; 61 const vec3_t actual_transformed = test_pose.TransformPoint(start_position); 62 EXPECT_VEC3_NEAR(expected_transformed, actual_transformed, tolerance); 63 } 64 } 65 66 TYPED_TEST(PoseTest, TransformVector) { 67 using quat_t = typename TestFixture::quat_t; 68 using vec3_t = typename TestFixture::vec3_t; 69 using Pose_t = typename TestFixture::Pose_t; 70 using FT = typename TestFixture::FT; 71 72 const auto tolerance = FT(0.0001); 73 74 const quat_t pose_rotation(Eigen::AngleAxis<FT>( 75 FT(M_PI / 6.0), vec3_t(FT(3.0), FT(4.0), FT(5.0)).normalized())); 76 77 const auto pose_position = vec3_t(FT(500.0), FT(-500.0), FT(300.0)); 78 79 const Pose_t test_pose(pose_rotation, pose_position); 80 81 for (int axis = 0; axis < 3; ++axis) { 82 vec3_t start_position = vec3_t::Zero(); 83 start_position[axis] = FT(1.0); 84 const vec3_t expected_rotated = pose_rotation * start_position; 85 const vec3_t actual_rotated = test_pose.Transform(start_position); 86 EXPECT_VEC3_NEAR(expected_rotated, actual_rotated, tolerance); 87 } 88 } 89 90 TYPED_TEST(PoseTest, Composition) { 91 using quat_t = typename TestFixture::quat_t; 92 using Pose_t = typename TestFixture::Pose_t; 93 using vec3_t = typename TestFixture::vec3_t; 94 using FT = typename TestFixture::FT; 95 96 const auto tolerance = FT(0.0001); 97 98 const quat_t first_rotation( 99 Eigen::AngleAxis<FT>(FT(M_PI / 2.0), vec3_t(FT(0.0), FT(0.0), FT(1.0)))); 100 const auto first_offset = vec3_t(FT(-3.0), FT(2.0), FT(-1.0)); 101 const quat_t second_rotation(Eigen::AngleAxis<FT>( 102 FT(M_PI / 3.0), vec3_t(FT(1.0), FT(-1.0), FT(0.0)).normalized())); 103 const auto second_offset = vec3_t(FT(6.0), FT(-7.0), FT(-8.0)); 104 105 const Pose_t first_pose(first_rotation, first_offset); 106 const Pose_t second_pose(second_rotation, second_offset); 107 108 const auto combined_pose(second_pose.Compose(first_pose)); 109 110 for (int axis = 0; axis < 3; ++axis) { 111 vec3_t start_position = vec3_t::Zero(); 112 start_position[axis] = FT(1.0); 113 const vec3_t expected_transformed = 114 second_pose.TransformPoint(first_pose.TransformPoint(start_position)); 115 const vec3_t actual_transformed = 116 combined_pose.TransformPoint(start_position); 117 EXPECT_VEC3_NEAR(expected_transformed, actual_transformed, tolerance); 118 } 119 } 120 121 TYPED_TEST(PoseTest, Inverse) { 122 using quat_t = typename TestFixture::quat_t; 123 using vec3_t = typename TestFixture::vec3_t; 124 using Pose_t = typename TestFixture::Pose_t; 125 using FT = typename TestFixture::FT; 126 127 const auto tolerance = FT(0.0001); 128 129 const quat_t pose_rotation(Eigen::AngleAxis<FT>( 130 FT(M_PI / 2.0), vec3_t(FT(4.0), FT(-2.0), FT(-1.0)).normalized())); 131 const auto pose_position = vec3_t(FT(-1.0), FT(2.0), FT(-4.0)); 132 133 Pose_t pose(pose_rotation, pose_position); 134 const Pose_t pose_inverse = pose.Inverse(); 135 136 for (int axis = 0; axis < 3; ++axis) { 137 vec3_t start_position = vec3_t::Zero(); 138 start_position[axis] = FT(1.0); 139 const vec3_t transformed = pose.Transform(start_position); 140 const vec3_t inverted = pose_inverse.Transform(transformed); 141 EXPECT_VEC3_NEAR(start_position, inverted, tolerance); 142 } 143 144 Pose_t nullified_pose[2] = { 145 pose.Compose(pose_inverse), pose_inverse.Compose(pose), 146 }; 147 148 for (int i = 0; i < 2; ++i) { 149 EXPECT_QUAT_NEAR(quat_t::Identity(), nullified_pose[i].GetRotation(), 150 tolerance); 151 EXPECT_VEC3_NEAR(vec3_t::Zero(), nullified_pose[i].GetPosition(), 152 tolerance); 153 } 154 } 155
