1 /*
2 * Copyright (C) 2021 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "media/Pose.h"
18
19 #include <gtest/gtest.h>
20
21 #include "media/QuaternionUtil.h"
22 #include "TestUtil.h"
23
24 using android::media::Pose3f;
25 using Eigen::Quaternionf;
26 using Eigen::Vector3f;
27
28 namespace android {
29 namespace media {
30 namespace {
31
TEST(Pose,CtorDefault)32 TEST(Pose, CtorDefault) {
33 Pose3f pose;
34 EXPECT_EQ(pose.translation(), Vector3f::Zero());
35 EXPECT_EQ(pose.rotation(), Quaternionf::Identity());
36 }
37
TEST(Pose,CtorRotation)38 TEST(Pose, CtorRotation) {
39 Quaternionf rot = Quaternionf::UnitRandom();
40 Pose3f pose(rot);
41 EXPECT_EQ(pose.translation(), Vector3f::Zero());
42 EXPECT_EQ(pose.rotation(), rot);
43 }
44
TEST(Pose,CtorTranslation)45 TEST(Pose, CtorTranslation) {
46 Vector3f trans{1, 2, 3};
47 Pose3f pose(trans);
48 EXPECT_EQ(pose.translation(), trans);
49 EXPECT_EQ(pose.rotation(), Quaternionf::Identity());
50 }
51
TEST(Pose,CtorTranslationRotation)52 TEST(Pose, CtorTranslationRotation) {
53 Quaternionf rot = Quaternionf::UnitRandom();
54 Vector3f trans{1, 2, 3};
55 Pose3f pose(trans, rot);
56 EXPECT_EQ(pose.translation(), trans);
57 EXPECT_EQ(pose.rotation(), rot);
58 }
59
TEST(Pose,Inverse)60 TEST(Pose, Inverse) {
61 Pose3f pose({1, 2, 3}, Quaternionf::UnitRandom());
62 EXPECT_EQ(pose.inverse() * pose, Pose3f());
63 EXPECT_EQ(pose * pose.inverse(), Pose3f());
64 }
65
TEST(Pose,IsApprox)66 TEST(Pose, IsApprox) {
67 constexpr float eps = std::numeric_limits<float>::epsilon();
68
69 EXPECT_EQ(Pose3f({1, 2, 3}, rotationVectorToQuaternion({4, 5, 6})),
70 Pose3f({1 + eps, 2 + eps, 3 + eps},
71 rotationVectorToQuaternion({4 + eps, 5 + eps, 6 + eps})));
72
73 EXPECT_NE(Pose3f({1, 2, 3}, rotationVectorToQuaternion({4, 5, 6})),
74 Pose3f({1.01, 2, 3}, rotationVectorToQuaternion({4, 5, 6})));
75
76 EXPECT_NE(Pose3f({1, 2, 3}, rotationVectorToQuaternion({4, 5, 6})),
77 Pose3f({1, 2, 3}, rotationVectorToQuaternion({4.01, 5, 6})));
78 }
79
TEST(Pose,Compose)80 TEST(Pose, Compose) {
81 Pose3f p1({1, 2, 3}, rotateZ(M_PI_2));
82 Pose3f p2({4, 5, 6}, rotateX(M_PI_2));
83 Pose3f p3({-4, 6, 9}, p1.rotation() * p2.rotation());
84 EXPECT_EQ(p1 * p2, p3);
85 }
86
TEST(Pose,MoveWithRateLimit_NoLimit)87 TEST(Pose, MoveWithRateLimit_NoLimit) {
88 Pose3f from({1, 1, 1}, Quaternionf::Identity());
89 Pose3f to({1, 1, 2}, rotateZ(M_PI_2));
90 auto result = moveWithRateLimit(from, to, 1, 10, 10);
91 EXPECT_EQ(std::get<0>(result), to);
92 EXPECT_FALSE(std::get<1>(result));
93 }
94
TEST(Pose,MoveWithRateLimit_TranslationLimit)95 TEST(Pose, MoveWithRateLimit_TranslationLimit) {
96 Pose3f from({1, 1, 1}, Quaternionf::Identity());
97 Pose3f to({1, 1, 2}, rotateZ(M_PI_2));
98 auto result = moveWithRateLimit(from, to, 1, 0.5f, 10);
99 Pose3f expected({1, 1, 1.5f}, rotateZ(M_PI_4));
100 EXPECT_EQ(std::get<0>(result), expected);
101 EXPECT_TRUE(std::get<1>(result));
102 }
103
TEST(Pose,MoveWithRateLimit_RotationLimit)104 TEST(Pose, MoveWithRateLimit_RotationLimit) {
105 Pose3f from({1, 1, 1}, Quaternionf::Identity());
106 Pose3f to({1, 1, 2}, rotateZ(M_PI_2));
107 auto result = moveWithRateLimit(from, to, 1, 10, M_PI_4);
108 Pose3f expected({1, 1, 1.5f}, rotateZ(M_PI_4));
109 EXPECT_EQ(std::get<0>(result), expected);
110 EXPECT_TRUE(std::get<1>(result));
111 }
112
TEST(Pose,FloatVectorRoundTrip1)113 TEST(Pose, FloatVectorRoundTrip1) {
114 // Rotation vector magnitude must be less than Pi.
115 std::vector<float> vec = { 1, 2, 3, 0.4, 0.5, 0.6};
116 std::optional<Pose3f> pose = Pose3f::fromVector(vec);
117 ASSERT_TRUE(pose.has_value());
118 std::vector<float> reconstructed = pose->toVector();
119 EXPECT_EQ(vec, reconstructed);
120 }
121
TEST(Pose,FloatVectorRoundTrip2)122 TEST(Pose, FloatVectorRoundTrip2) {
123 Pose3f pose({1, 2, 3}, Quaternionf::UnitRandom());
124 std::vector<float> vec = pose.toVector();
125 std::optional<Pose3f> reconstructed = Pose3f::fromVector(vec);
126 ASSERT_TRUE(reconstructed.has_value());
127 EXPECT_EQ(pose, reconstructed.value());
128 }
129
TEST(Pose,FloatVectorInvalid)130 TEST(Pose, FloatVectorInvalid) {
131 EXPECT_FALSE(Pose3f::fromVector({}).has_value());
132 EXPECT_FALSE(Pose3f::fromVector({1, 2, 3, 4, 5}).has_value());
133 EXPECT_FALSE(Pose3f::fromVector({1, 2, 3, 4, 5, 6, 7}).has_value());
134 }
135
136 } // namespace
137 } // namespace media
138 } // namespace android
139