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
7using PoseTypes = ::testing::Types<float, double>;
8
9template <class T>
10class 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
19TYPED_TEST_CASE(PoseTest, PoseTypes);
20
21// Check that the two matrix methods are inverses of each other
22TYPED_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
42TYPED_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
66TYPED_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
90TYPED_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
121TYPED_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