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 #pragma once
17 
18 #include <optional>
19 
20 #include "media/Pose.h"
21 
22 namespace android {
23 namespace media {
24 
25 /**
26  * Drift compensator for a stream of poses.
27  *
28  * This is effectively a high-pass filter for a pose stream, removing any DC-offset / bias. The
29  * provided input stream will be "pulled" toward identity with an exponential decay filter with a
30  * configurable time constant. Rotation and translation are handled separately.
31  *
32  * Typical usage:
33  * PoseDriftCompensator comp(...);
34  *
35  * while (...) {
36  *   comp.setInput(...);
37  *   Pose3f output = comp.getOutput();
38  * }
39  *
40  * There doesn't need to be a 1:1 correspondence between setInput() and getOutput() calls. The
41  * output timestamp is always that of the last setInput() call. Calling recenter() will reset the
42  * bias to the current output, causing the output to be identity.
43  *
44  * The initial bias point is identity.
45  *
46  * This implementation is thread-compatible, but not thread-safe.
47  */
48 class PoseDriftCompensator {
49   public:
50     struct Options {
51         float translationalDriftTimeConstant = std::numeric_limits<float>::infinity();
52         float rotationalDriftTimeConstant = std::numeric_limits<float>::infinity();
53     };
54 
55     explicit PoseDriftCompensator(const Options& options);
56 
57     void setInput(int64_t timestamp, const Pose3f& input);
58 
59     void recenter();
60 
61     Pose3f getOutput() const;
62 
63   private:
64     const Options mOptions;
65 
66     Pose3f mPrevInput;
67     Pose3f mOutput;
68     std::optional<int64_t> mTimestamp;
69 
70     Pose3f scale(const Pose3f& pose, int64_t dt);
71 };
72 
73 }  // namespace media
74 }  // namespace android
75