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
TYPED_TEST(PoseTest,SelfInverse)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
TYPED_TEST(PoseTest,TransformPoint)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
TYPED_TEST(PoseTest,TransformVector)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
TYPED_TEST(PoseTest,Composition)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
TYPED_TEST(PoseTest,Inverse)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