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 #pragma once 18 19 #include <string> 20 21 #include <audio_utils/Statistics.h> 22 #include <media/Pose.h> 23 24 namespace android::media { 25 26 /** 27 * PosePredictorVerifier is used to validate predictions 28 * 29 * This class is not thread-safe 30 */ 31 class PosePredictorVerifier { 32 public: toString()33 std::string toString() const { 34 return mErrorStats.toString(); 35 } 36 37 static constexpr int64_t kMillisToNanos = 1000000; 38 verifyActualPose(int64_t timestampNs,const Pose3f & pose)39 void verifyActualPose(int64_t timestampNs, const Pose3f& pose) { 40 for (auto it = mPredictions.begin(); it != mPredictions.end();) { 41 if (it->first < timestampNs) { 42 it = mPredictions.erase(it); 43 } else { 44 int64_t dt = it->first - timestampNs; 45 if (std::abs(dt) < 10 * kMillisToNanos) { 46 const float angle = pose.rotation().angularDistance(it->second.rotation()); 47 const float error = std::abs(angle); // L1 (absolute difference) here. 48 mLastError = error; 49 mErrorStats.add(error); 50 } 51 break; 52 } 53 } 54 } 55 addPredictedPose(int64_t atNs,const Pose3f & pose)56 void addPredictedPose(int64_t atNs, const Pose3f& pose) { 57 mPredictions.emplace_back(atNs, pose); 58 } 59 lastError()60 float lastError() const { 61 return mLastError; 62 } 63 cumulativeAverageError()64 float cumulativeAverageError() const { 65 return mErrorStats.getMean(); 66 } 67 68 private: 69 static constexpr double kCumulativeErrorAlpha = 0.999; 70 std::deque<std::pair<int64_t, Pose3f>> mPredictions; 71 float mLastError{}; 72 android::audio_utils::Statistics<double> mErrorStats{kCumulativeErrorAlpha}; 73 }; 74 75 } // namespace androd::media 76