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 "PoseDriftCompensator.h"
18
19 #include <cmath>
20
21 #include "media/QuaternionUtil.h"
22
23 namespace android {
24 namespace media {
25
26 using Eigen::Quaternionf;
27 using Eigen::Vector3f;
28
PoseDriftCompensator(const Options & options)29 PoseDriftCompensator::PoseDriftCompensator(const Options& options) : mOptions(options) {}
30
setInput(int64_t timestamp,const Pose3f & input)31 void PoseDriftCompensator::setInput(int64_t timestamp, const Pose3f& input) {
32 if (mTimestamp.has_value()) {
33 // Avoid computation upon first input (only sets the initial state).
34 Pose3f prevInputToInput = mPrevInput.inverse() * input;
35 mOutput = scale(mOutput, timestamp - mTimestamp.value()) * prevInputToInput;
36 }
37 mPrevInput = input;
38 mTimestamp = timestamp;
39 }
40
recenter()41 void PoseDriftCompensator::recenter() {
42 mTimestamp.reset();
43 mOutput = Pose3f();
44 }
45
getOutput() const46 Pose3f PoseDriftCompensator::getOutput() const {
47 return mOutput;
48 }
49
scale(const Pose3f & pose,int64_t dt)50 Pose3f PoseDriftCompensator::scale(const Pose3f& pose, int64_t dt) {
51 // Translation.
52 Vector3f translation = pose.translation();
53 translation *= std::expf(-static_cast<float>(dt) / mOptions.translationalDriftTimeConstant);
54
55 // Rotation.
56 Vector3f rotationVec = quaternionToRotationVector(pose.rotation());
57 rotationVec *= std::expf(-static_cast<float>(dt) / mOptions.rotationalDriftTimeConstant);
58
59 return Pose3f(translation, rotationVectorToQuaternion(rotationVec));
60 }
61
62 } // namespace media
63 } // namespace android
64