1 /*
2 * Copyright (C) 2023 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 "PosePredictor.h"
18
19 namespace android::media {
20
21 namespace {
22 #ifdef ENABLE_VERIFICATION
23 constexpr bool kEnableVerification = true;
24 constexpr std::array<int, 3> kLookAheadMs{ 50, 100, 200 };
25 #else
26 constexpr bool kEnableVerification = false;
27 constexpr std::array<int, 0> kLookAheadMs{};
28 #endif
29
30 } // namespace
31
add(int64_t atNs,const Pose3f & pose,const Twist3f & twist)32 void LeastSquaresPredictor::add(int64_t atNs, const Pose3f& pose, const Twist3f& twist)
33 {
34 (void)twist;
35 mLastAtNs = atNs;
36 mLastPose = pose;
37 const auto q = pose.rotation();
38 const double datNs = static_cast<double>(atNs);
39 mRw.add({datNs, q.w()});
40 mRx.add({datNs, q.x()});
41 mRy.add({datNs, q.y()});
42 mRz.add({datNs, q.z()});
43 }
44
predict(int64_t atNs) const45 Pose3f LeastSquaresPredictor::predict(int64_t atNs) const
46 {
47 if (mRw.getN() < kMinimumSamplesForPrediction) return mLastPose;
48
49 /*
50 * Using parametric form, we have q(t) = { w(t), x(t), y(t), z(t) }.
51 * We compute the least squares prediction of w, x, y, z.
52 */
53 const double dLookahead = static_cast<double>(atNs);
54 Eigen::Quaternionf lsq(
55 mRw.getYFromX(dLookahead),
56 mRx.getYFromX(dLookahead),
57 mRy.getYFromX(dLookahead),
58 mRz.getYFromX(dLookahead));
59
60 /*
61 * We cheat here, since the result lsq is the least squares prediction
62 * in H (arbitrary quaternion), not the least squares prediction in
63 * SO(3) (unit quaternion).
64 *
65 * In other words, the result for lsq is most likely not a unit quaternion.
66 * To solve this, we normalize, thereby selecting the closest unit quaternion
67 * in SO(3) to the prediction in H.
68 */
69 lsq.normalize();
70 return Pose3f(lsq);
71 }
72
reset()73 void LeastSquaresPredictor::reset() {
74 mLastAtNs = {};
75 mLastPose = {};
76 mRw.reset();
77 mRx.reset();
78 mRy.reset();
79 mRz.reset();
80 }
81
toString(size_t index) const82 std::string LeastSquaresPredictor::toString(size_t index) const {
83 std::string s(index, ' ');
84 s.append("LeastSquaresPredictor using alpha: ")
85 .append(std::to_string(mAlpha))
86 .append(" last pose: ")
87 .append(mLastPose.toString())
88 .append("\n");
89 return s;
90 }
91
92 // Formatting
createDelimiterIdx(size_t predictors,size_t lookaheads)93 static inline std::vector<size_t> createDelimiterIdx(size_t predictors, size_t lookaheads) {
94 if (lookaheads == 0) return {};
95 --lookaheads;
96 std::vector<size_t> delimiterIdx(lookaheads);
97 for (size_t i = 0; i < lookaheads; ++i) {
98 delimiterIdx[i] = (i + 1) * predictors;
99 }
100 return delimiterIdx;
101 }
102
PosePredictor()103 PosePredictor::PosePredictor()
104 : mPredictors{
105 // First predictors must match switch in getCurrentPredictor()
106 std::make_shared<LastPredictor>(),
107 std::make_shared<TwistPredictor>(),
108 std::make_shared<LeastSquaresPredictor>(),
109 // After this, can place additional predictors here for comparison such as
110 // std::make_shared<LeastSquaresPredictor>(0.25),
111 }
112 , mLookaheadMs(kLookAheadMs.begin(), kLookAheadMs.end())
113 , mVerifiers(std::size(mLookaheadMs) * std::size(mPredictors))
114 , mDelimiterIdx(createDelimiterIdx(std::size(mPredictors), std::size(mLookaheadMs)))
115 , mPredictionRecorder(
116 std::size(mVerifiers) /* vectorSize */, std::chrono::seconds(1), 10 /* maxLogLine */,
117 mDelimiterIdx)
118 , mPredictionDurableRecorder(
119 std::size(mVerifiers) /* vectorSize */, std::chrono::minutes(1), 10 /* maxLogLine */,
120 mDelimiterIdx)
121 {
122 }
123
predict(int64_t timestampNs,const Pose3f & pose,const Twist3f & twist,float predictionDurationNs)124 Pose3f PosePredictor::predict(
125 int64_t timestampNs, const Pose3f& pose, const Twist3f& twist, float predictionDurationNs)
126 {
127 if (timestampNs - mLastTimestampNs > kMaximumSampleIntervalBeforeResetNs) {
128 for (const auto& predictor : mPredictors) {
129 predictor->reset();
130 }
131 ++mResets;
132 }
133 mLastTimestampNs = timestampNs;
134
135 auto selectedPredictor = getCurrentPredictor();
136 if constexpr (kEnableVerification) {
137 // Update all Predictors
138 for (const auto& predictor : mPredictors) {
139 predictor->add(timestampNs, pose, twist);
140 }
141
142 // Update Verifiers and calculate errors
143 std::vector<float> error(std::size(mVerifiers));
144 for (size_t i = 0; i < mLookaheadMs.size(); ++i) {
145 constexpr float RADIAN_TO_DEGREES = 180 / M_PI;
146 const int64_t atNs =
147 timestampNs + mLookaheadMs[i] * PosePredictorVerifier::kMillisToNanos;
148
149 for (size_t j = 0; j < mPredictors.size(); ++j) {
150 const size_t idx = i * std::size(mPredictors) + j;
151 mVerifiers[idx].verifyActualPose(timestampNs, pose);
152 mVerifiers[idx].addPredictedPose(atNs, mPredictors[j]->predict(atNs));
153 error[idx] = RADIAN_TO_DEGREES * mVerifiers[idx].lastError();
154 }
155 }
156 // Record errors
157 mPredictionRecorder.record(error);
158 mPredictionDurableRecorder.record(error);
159 } else /* constexpr */ {
160 selectedPredictor->add(timestampNs, pose, twist);
161 }
162
163 // Deliver prediction
164 const int64_t predictionTimeNs = timestampNs + (int64_t)predictionDurationNs;
165 return selectedPredictor->predict(predictionTimeNs);
166 }
167
setPosePredictorType(PosePredictorType type)168 void PosePredictor::setPosePredictorType(PosePredictorType type) {
169 if (!isValidPosePredictorType(type)) return;
170 if (type == mSetType) return;
171 mSetType = type;
172 if (type == android::media::PosePredictorType::AUTO) {
173 type = android::media::PosePredictorType::LEAST_SQUARES;
174 }
175 if (type != mCurrentType) {
176 mCurrentType = type;
177 if constexpr (!kEnableVerification) {
178 // Verification keeps all predictors up-to-date.
179 // If we don't enable verification, we must reset the current predictor.
180 getCurrentPredictor()->reset();
181 }
182 }
183 }
184
toString(size_t index) const185 std::string PosePredictor::toString(size_t index) const {
186 std::string prefixSpace(index, ' ');
187 std::string ss(prefixSpace);
188 ss.append("PosePredictor:\n")
189 .append(prefixSpace)
190 .append(" Current Prediction Type: ")
191 .append(android::media::toString(mCurrentType))
192 .append("\n")
193 .append(prefixSpace)
194 .append(" Resets: ")
195 .append(std::to_string(mResets))
196 .append("\n")
197 .append(getCurrentPredictor()->toString(index + 1));
198 if constexpr (kEnableVerification) {
199 // dump verification
200 ss.append(prefixSpace)
201 .append(" Prediction abs error (L1) degrees [ type (");
202 for (size_t i = 0; i < mPredictors.size(); ++i) {
203 if (i > 0) ss.append(" , ");
204 ss.append(mPredictors[i]->name());
205 }
206 ss.append(" ) x ( ");
207 for (size_t i = 0; i < mLookaheadMs.size(); ++i) {
208 if (i > 0) ss.append(" : ");
209 ss.append(std::to_string(mLookaheadMs[i]));
210 }
211 std::vector<float> cumulativeAverageErrors(std::size(mVerifiers));
212 for (size_t i = 0; i < cumulativeAverageErrors.size(); ++i) {
213 cumulativeAverageErrors[i] = mVerifiers[i].cumulativeAverageError();
214 }
215 ss.append(" ) ms ]\n")
216 .append(prefixSpace)
217 .append(" Cumulative Average Error:\n")
218 .append(prefixSpace)
219 .append(" ")
220 .append(VectorRecorder::toString(cumulativeAverageErrors, mDelimiterIdx, "%.3g"))
221 .append("\n")
222 .append(prefixSpace)
223 .append(" PerMinuteHistory:\n")
224 .append(mPredictionDurableRecorder.toString(index + 3))
225 .append(prefixSpace)
226 .append(" PerSecondHistory:\n")
227 .append(mPredictionRecorder.toString(index + 3));
228 }
229 return ss;
230 }
231
getCurrentPredictor() const232 std::shared_ptr<PredictorBase> PosePredictor::getCurrentPredictor() const {
233 // we don't use a map here, we look up directly
234 switch (mCurrentType) {
235 default:
236 case android::media::PosePredictorType::LAST:
237 return mPredictors[0];
238 case android::media::PosePredictorType::TWIST:
239 return mPredictors[1];
240 case android::media::PosePredictorType::AUTO: // shouldn't occur here.
241 case android::media::PosePredictorType::LEAST_SQUARES:
242 return mPredictors[2];
243 }
244 }
245
246 } // namespace android::media
247