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 <gtest/gtest.h>
18 #include <cmath>
19
20 #include "PoseDriftCompensator.h"
21 #include "QuaternionUtil.h"
22 #include "TestUtil.h"
23
24 namespace android {
25 namespace media {
26 namespace {
27
28 using Eigen::Quaternionf;
29 using Eigen::Vector3f;
30 using Options = PoseDriftCompensator::Options;
31
TEST(PoseDriftCompensator,Initial)32 TEST(PoseDriftCompensator, Initial) {
33 PoseDriftCompensator comp(Options{});
34 EXPECT_EQ(comp.getOutput(), Pose3f());
35 }
36
TEST(PoseDriftCompensator,NoDrift)37 TEST(PoseDriftCompensator, NoDrift) {
38 Pose3f pose1({1, 2, 3}, Quaternionf::UnitRandom());
39 Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());
40 PoseDriftCompensator comp(Options{});
41
42 // First pose sets the baseline.
43 comp.setInput(1000, pose1);
44 EXPECT_EQ(comp.getOutput(), Pose3f());
45
46 comp.setInput(2000, pose2);
47 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
48
49 // Recentering resets the baseline.
50 comp.recenter();
51 EXPECT_EQ(comp.getOutput(), Pose3f());
52
53 comp.setInput(3000, pose1);
54 EXPECT_EQ(comp.getOutput(), Pose3f());
55
56 comp.setInput(4000, pose2);
57 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
58 }
59
TEST(PoseDriftCompensator,NoDriftZeroTime)60 TEST(PoseDriftCompensator, NoDriftZeroTime) {
61 Pose3f pose1({1, 2, 3}, Quaternionf::UnitRandom());
62 Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());
63 PoseDriftCompensator comp(Options{});
64
65 comp.setInput(1000, pose1);
66 EXPECT_EQ(comp.getOutput(), Pose3f());
67
68 comp.setInput(1000, pose2);
69 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
70
71 comp.recenter();
72 EXPECT_EQ(comp.getOutput(), Pose3f());
73
74 comp.setInput(1000, pose1);
75 EXPECT_EQ(comp.getOutput(), Pose3f());
76
77 comp.setInput(1000, pose2);
78 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
79 }
80
TEST(PoseDriftCompensator,Asymptotic)81 TEST(PoseDriftCompensator, Asymptotic) {
82 Pose3f pose({1, 2, 3}, Quaternionf::UnitRandom());
83
84 PoseDriftCompensator comp(
85 Options{.translationalDriftTimeConstant = 1, .rotationalDriftTimeConstant = 1});
86
87 // Set the same pose for a long time.
88 for (int64_t t = 0; t < 1000; ++t) {
89 comp.setInput(t, pose);
90 }
91
92 // Output would have faded to approx. identity.
93 EXPECT_EQ(comp.getOutput(), Pose3f());
94 }
95
TEST(PoseDriftCompensator,Fast)96 TEST(PoseDriftCompensator, Fast) {
97 Pose3f pose1({1, 2, 3}, Quaternionf::UnitRandom());
98 Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());
99 PoseDriftCompensator comp(
100 Options{.translationalDriftTimeConstant = 1e7, .rotationalDriftTimeConstant = 1e7});
101
102 comp.setInput(0, pose1);
103 EXPECT_EQ(comp.getOutput(), Pose3f());
104
105 comp.setInput(1, pose2);
106 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
107
108 comp.recenter();
109 EXPECT_EQ(comp.getOutput(), Pose3f());
110
111 comp.setInput(2, pose1);
112 EXPECT_EQ(comp.getOutput(), Pose3f());
113
114 comp.setInput(3, pose2);
115 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
116 }
117
TEST(PoseDriftCompensator,Drift)118 TEST(PoseDriftCompensator, Drift) {
119 Pose3f pose1({1, 2, 3}, rotateZ(-M_PI * 3 / 4));
120 PoseDriftCompensator comp(
121 Options{.translationalDriftTimeConstant = 500, .rotationalDriftTimeConstant = 1000});
122
123 // Establish a baseline.
124 comp.setInput(1000, Pose3f());
125
126 // Initial pose is used as is.
127 comp.setInput(1000, pose1);
128 EXPECT_EQ(comp.getOutput(), pose1);
129
130 // After 1000 ticks, our rotation should be exp(-1) and translation exp(-2) from identity.
131 comp.setInput(2000, pose1);
132 EXPECT_EQ(comp.getOutput(),
133 Pose3f(Vector3f{1, 2, 3} * std::expf(-2), rotateZ(-M_PI * 3 / 4 * std::expf(-1))));
134
135 // As long as the input stays the same, we'll continue to advance towards identity.
136 comp.setInput(3000, pose1);
137 EXPECT_EQ(comp.getOutput(),
138 Pose3f(Vector3f{1, 2, 3} * std::expf(-4), rotateZ(-M_PI * 3 / 4 * std::expf(-2))));
139
140 comp.recenter();
141 EXPECT_EQ(comp.getOutput(), Pose3f());
142 }
143
144 } // namespace
145 } // namespace media
146 } // namespace android
147