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. 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. 25 Pose(Eigen::Quaternion<T> rotation, Eigen::Vector3<T> position) 26 : rotation_(rotation), position_(position) {} 27 28 void Invert() { 29 rotation_ = rotation_.inverse(); 30 position_ = rotation_ * -position_; 31 } 32 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 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 47 Pose Compose(const Pose& other) const { 48 Pose result(*this); 49 result.ComposeInPlace(other); 50 return result; 51 } 52 53 Eigen::Vector3<T> TransformPoint(const Eigen::Vector3<T>& v) const { 54 return rotation_ * v + position_; 55 } 56 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. 69 Eigen::Quaternion<T> GetRotation() const { return rotation_; } 70 71 // Gets the position of the 3D pose. 72 Eigen::Vector3<T> GetPosition() const { return position_; } 73 74 // Sets the rotation of the 3D pose. 75 void SetRotation(Eigen::Quaternion<T> rotation) { rotation_ = rotation; } 76 77 // Sets the position of the 3D pose. 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> 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> 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
