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