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