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 #include <string>
20 #include <vector>
21 #include <Eigen/Geometry>
22
23 namespace android {
24 namespace media {
25
26 /**
27 * A 6-DoF pose.
28 * This class represents a proper rigid transformation (translation + rotation) between a reference
29 * frame and a target frame,
30 *
31 * See https://en.wikipedia.org/wiki/Six_degrees_of_freedom
32 */
33 class Pose3f {
34 public:
35 /** Typical precision for isApprox comparisons. */
36 static constexpr float kDummyPrecision = 1e-5f;
37
Pose3f(const Eigen::Vector3f & translation,const Eigen::Quaternionf & rotation)38 Pose3f(const Eigen::Vector3f& translation, const Eigen::Quaternionf& rotation)
39 : mTranslation(translation), mRotation(rotation) {}
40
Pose3f(const Eigen::Vector3f & translation)41 explicit Pose3f(const Eigen::Vector3f& translation)
42 : Pose3f(translation, Eigen::Quaternionf::Identity()) {}
43
Pose3f(const Eigen::Quaternionf & rotation)44 explicit Pose3f(const Eigen::Quaternionf& rotation)
45 : Pose3f(Eigen::Vector3f::Zero(), rotation) {}
46
Pose3f()47 Pose3f() : Pose3f(Eigen::Vector3f::Zero(), Eigen::Quaternionf::Identity()) {}
48
Pose3f(const Pose3f & other)49 Pose3f(const Pose3f& other) { *this = other; }
50
51 /**
52 * Create instance from a vector-of-floats representation.
53 * The vector is expected to have exactly 6 elements, where the first three are a translation
54 * vector and the last three are a rotation vector.
55 *
56 * Returns nullopt if the input vector is illegal.
57 */
58 static std::optional<Pose3f> fromVector(const std::vector<float>& vec);
59
60 /**
61 * Convert instance to a vector-of-floats representation.
62 * The vector will have exactly 6 elements, where the first three are a translation vector and
63 * the last three are a rotation vector.
64 */
65 std::vector<float> toVector() const;
66
67 // Convert instance to a string representation.
68 std::string toString() const;
69
70 Pose3f& operator=(const Pose3f& other) {
71 mTranslation = other.mTranslation;
72 mRotation = other.mRotation;
73 return *this;
74 }
75
translation()76 Eigen::Vector3f translation() const { return mTranslation; };
rotation()77 Eigen::Quaternionf rotation() const { return mRotation; };
78
79 /**
80 * Reverses the reference and target frames.
81 */
inverse()82 Pose3f inverse() const {
83 Eigen::Quaternionf invRotation = mRotation.inverse();
84 return Pose3f(-(invRotation * translation()), invRotation);
85 }
86
87 /**
88 * Composes (chains) together two poses. By convention, this only makes sense if the target
89 * frame of the left-hand pose is the same the reference frame of the right-hand pose.
90 * Note that this operator is not commutative.
91 */
92 Pose3f operator*(const Pose3f& other) const {
93 Pose3f result = *this;
94 result *= other;
95 return result;
96 }
97
98 Pose3f& operator*=(const Pose3f& other) {
99 mTranslation += mRotation * other.mTranslation;
100 mRotation *= other.mRotation;
101 return *this;
102 }
103
104 /**
105 * This is an imprecise "fuzzy" comparison, which is only to be used for validity-testing
106 * purposes.
107 */
108 bool isApprox(const Pose3f& other, float prec = kDummyPrecision) const {
109 return (mTranslation - other.mTranslation).norm() < prec &&
110 // Quaternions are equivalent under sign inversion.
111 ((mRotation.coeffs() - other.mRotation.coeffs()).norm() < prec ||
112 (mRotation.coeffs() + other.mRotation.coeffs()).norm() < prec);
113 }
114
115 private:
116 Eigen::Vector3f mTranslation;
117 Eigen::Quaternionf mRotation;
118 };
119
120 /**
121 * Pretty-printer for Pose3f.
122 */
123 std::ostream& operator<<(std::ostream& os, const Pose3f& pose);
124
125 /**
126 * Move between the 'from' pose and the 'to' pose, while making sure velocity limits are enforced.
127 * If velocity limits are not violated, returns the 'to' pose and false.
128 * If velocity limits are violated, returns pose farthest along the path that can be reached within
129 * the limits, and true.
130 */
131 std::tuple<Pose3f, bool> moveWithRateLimit(const Pose3f& from, const Pose3f& to, float t,
132 float maxTranslationalVelocity,
133 float maxRotationalVelocity);
134
135 template <typename T>
nsToFloatMs(T ns)136 static float nsToFloatMs(T ns) {
137 return ns * 1e-6f;
138 }
139
140 } // namespace media
141 } // namespace android
142