1 #ifndef ANDROID_DVR_POSE_H_
2 #define ANDROID_DVR_POSE_H_
3
4 #include <private/dvr/eigen.h>
5
6 namespace android {
7 namespace dvr {
8
9 // Encapsulates a 3D pose (rotation and position).
10 //
11 // @tparam T Data type for storing the position coordinate and rotation
12 // quaternion.
13 template <typename T>
14 class Pose {
15 public:
16 // Creates identity pose.
Pose()17 Pose()
18 : rotation_(Eigen::Quaternion<T>::Identity()),
19 position_(Eigen::Vector3<T>::Zero()) {}
20
21 // Initializes a pose with given rotation and position.
22 //
23 // rotation Initial rotation.
24 // position Initial position.
Pose(Eigen::Quaternion<T> rotation,Eigen::Vector3<T> position)25 Pose(Eigen::Quaternion<T> rotation, Eigen::Vector3<T> position)
26 : rotation_(rotation), position_(position) {}
27
Invert()28 void Invert() {
29 rotation_ = rotation_.inverse();
30 position_ = rotation_ * -position_;
31 }
32
Inverse()33 Pose Inverse() const {
34 Pose result(*this);
35 result.Invert();
36 return result;
37 }
38
39 // Compute the composition of this pose with another, storing the result
40 // in the current object
ComposeInPlace(const Pose & other)41 void ComposeInPlace(const Pose& other) {
42 position_ = position_ + rotation_ * other.position_;
43 rotation_ = rotation_ * other.rotation_;
44 }
45
46 // Computes the composition of this pose with another, and returns the result
Compose(const Pose & other)47 Pose Compose(const Pose& other) const {
48 Pose result(*this);
49 result.ComposeInPlace(other);
50 return result;
51 }
52
TransformPoint(const Eigen::Vector3<T> & v)53 Eigen::Vector3<T> TransformPoint(const Eigen::Vector3<T>& v) const {
54 return rotation_ * v + position_;
55 }
56
Transform(const Eigen::Vector3<T> & v)57 Eigen::Vector3<T> Transform(const Eigen::Vector3<T>& v) const {
58 return rotation_ * v;
59 }
60
61 Pose& operator*=(const Pose& other) {
62 ComposeInPlace(other);
63 return *this;
64 }
65
66 Pose operator*(const Pose& other) const { return Compose(other); }
67
68 // Gets the rotation of the 3D pose.
GetRotation()69 Eigen::Quaternion<T> GetRotation() const { return rotation_; }
70
71 // Gets the position of the 3D pose.
GetPosition()72 Eigen::Vector3<T> GetPosition() const { return position_; }
73
74 // Sets the rotation of the 3D pose.
SetRotation(Eigen::Quaternion<T> rotation)75 void SetRotation(Eigen::Quaternion<T> rotation) { rotation_ = rotation; }
76
77 // Sets the position of the 3D pose.
SetPosition(Eigen::Vector3<T> position)78 void SetPosition(Eigen::Vector3<T> position) { position_ = position; }
79
80 // Gets a 4x4 matrix representing a transform from the reference space (that
81 // the rotation and position of the pose are relative to) to the object space.
82 Eigen::AffineMatrix<T, 4> GetObjectFromReferenceMatrix() const;
83
84 // Gets a 4x4 matrix representing a transform from the object space to the
85 // reference space (that the rotation and position of the pose are relative
86 // to).
87 Eigen::AffineMatrix<T, 4> GetReferenceFromObjectMatrix() const;
88
89 private:
90 Eigen::Quaternion<T> rotation_;
91 Eigen::Vector3<T> position_;
92 };
93
94 template <typename T>
GetObjectFromReferenceMatrix()95 Eigen::AffineMatrix<T, 4> Pose<T>::GetObjectFromReferenceMatrix() const {
96 // The transfrom from the reference is the inverse of the pose.
97 Eigen::AffineMatrix<T, 4> matrix(rotation_.inverse().toRotationMatrix());
98 return matrix.translate(-position_);
99 }
100
101 template <typename T>
GetReferenceFromObjectMatrix()102 Eigen::AffineMatrix<T, 4> Pose<T>::GetReferenceFromObjectMatrix() const {
103 // The transfrom to the reference.
104 Eigen::AffineMatrix<T, 4> matrix(rotation_.toRotationMatrix());
105 return matrix.pretranslate(position_);
106 }
107
108 //------------------------------------------------------------------------------
109 // Type-specific typedefs.
110 //------------------------------------------------------------------------------
111
112 using Posef = Pose<float>;
113 using Posed = Pose<double>;
114
115 } // namespace dvr
116 } // namespace android
117
118 #endif // ANDROID_DVR_POSE_H_
119