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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
|
#include <gtest/gtest.h>
#include <private/dvr/eigen.h>
#include <private/dvr/pose.h>
#include <private/dvr/test/test_macros.h>
using PoseTypes = ::testing::Types<float, double>;
template <class T>
class PoseTest : public ::testing::TestWithParam<T> {
public:
using FT = T;
using Pose_t = android::dvr::Pose<FT>;
using quat_t = Eigen::Quaternion<FT>;
using vec3_t = Eigen::Vector3<FT>;
using mat4_t = Eigen::AffineMatrix<FT, 4>;
};
TYPED_TEST_CASE(PoseTest, PoseTypes);
// Check that the two matrix methods are inverses of each other
TYPED_TEST(PoseTest, SelfInverse) {
using quat_t = typename TestFixture::quat_t;
using vec3_t = typename TestFixture::vec3_t;
using Pose_t = typename TestFixture::Pose_t;
using mat4_t = typename TestFixture::mat4_t;
using FT = typename TestFixture::FT;
const auto tolerance = FT(0.0001);
const quat_t initial_rotation(Eigen::AngleAxis<FT>(
FT(M_PI / 3.0), vec3_t(FT(3.0), FT(4.0), FT(5.0)).normalized()));
const vec3_t initial_position = vec3_t(FT(2.0), FT(10.0), FT(-4.0));
const Pose_t initial_pose(initial_rotation, initial_position);
auto result_pose = initial_pose.GetReferenceFromObjectMatrix() *
initial_pose.GetObjectFromReferenceMatrix();
EXPECT_MAT4_NEAR(result_pose, mat4_t::Identity(), tolerance);
}
TYPED_TEST(PoseTest, TransformPoint) {
using quat_t = typename TestFixture::quat_t;
using vec3_t = typename TestFixture::vec3_t;
using Pose_t = typename TestFixture::Pose_t;
using FT = typename TestFixture::FT;
const auto tolerance = FT(0.0001);
const quat_t pose_rotation(
Eigen::AngleAxis<FT>(FT(M_PI / 2.0), vec3_t(FT(0.0), FT(0.0), FT(1.0))));
const auto pose_position = vec3_t(FT(1.0), FT(1.0), FT(2.0));
const Pose_t test_pose(pose_rotation, pose_position);
for (int axis = 0; axis < 3; ++axis) {
vec3_t start_position = vec3_t::Zero();
start_position[axis] = FT(1.0);
const vec3_t expected_transformed =
(pose_rotation * start_position) + pose_position;
const vec3_t actual_transformed = test_pose.TransformPoint(start_position);
EXPECT_VEC3_NEAR(expected_transformed, actual_transformed, tolerance);
}
}
TYPED_TEST(PoseTest, TransformVector) {
using quat_t = typename TestFixture::quat_t;
using vec3_t = typename TestFixture::vec3_t;
using Pose_t = typename TestFixture::Pose_t;
using FT = typename TestFixture::FT;
const auto tolerance = FT(0.0001);
const quat_t pose_rotation(Eigen::AngleAxis<FT>(
FT(M_PI / 6.0), vec3_t(FT(3.0), FT(4.0), FT(5.0)).normalized()));
const auto pose_position = vec3_t(FT(500.0), FT(-500.0), FT(300.0));
const Pose_t test_pose(pose_rotation, pose_position);
for (int axis = 0; axis < 3; ++axis) {
vec3_t start_position = vec3_t::Zero();
start_position[axis] = FT(1.0);
const vec3_t expected_rotated = pose_rotation * start_position;
const vec3_t actual_rotated = test_pose.Transform(start_position);
EXPECT_VEC3_NEAR(expected_rotated, actual_rotated, tolerance);
}
}
TYPED_TEST(PoseTest, Composition) {
using quat_t = typename TestFixture::quat_t;
using Pose_t = typename TestFixture::Pose_t;
using vec3_t = typename TestFixture::vec3_t;
using FT = typename TestFixture::FT;
const auto tolerance = FT(0.0001);
const quat_t first_rotation(
Eigen::AngleAxis<FT>(FT(M_PI / 2.0), vec3_t(FT(0.0), FT(0.0), FT(1.0))));
const auto first_offset = vec3_t(FT(-3.0), FT(2.0), FT(-1.0));
const quat_t second_rotation(Eigen::AngleAxis<FT>(
FT(M_PI / 3.0), vec3_t(FT(1.0), FT(-1.0), FT(0.0)).normalized()));
const auto second_offset = vec3_t(FT(6.0), FT(-7.0), FT(-8.0));
const Pose_t first_pose(first_rotation, first_offset);
const Pose_t second_pose(second_rotation, second_offset);
const auto combined_pose(second_pose.Compose(first_pose));
for (int axis = 0; axis < 3; ++axis) {
vec3_t start_position = vec3_t::Zero();
start_position[axis] = FT(1.0);
const vec3_t expected_transformed =
second_pose.TransformPoint(first_pose.TransformPoint(start_position));
const vec3_t actual_transformed =
combined_pose.TransformPoint(start_position);
EXPECT_VEC3_NEAR(expected_transformed, actual_transformed, tolerance);
}
}
TYPED_TEST(PoseTest, Inverse) {
using quat_t = typename TestFixture::quat_t;
using vec3_t = typename TestFixture::vec3_t;
using Pose_t = typename TestFixture::Pose_t;
using FT = typename TestFixture::FT;
const auto tolerance = FT(0.0001);
const quat_t pose_rotation(Eigen::AngleAxis<FT>(
FT(M_PI / 2.0), vec3_t(FT(4.0), FT(-2.0), FT(-1.0)).normalized()));
const auto pose_position = vec3_t(FT(-1.0), FT(2.0), FT(-4.0));
Pose_t pose(pose_rotation, pose_position);
const Pose_t pose_inverse = pose.Inverse();
for (int axis = 0; axis < 3; ++axis) {
vec3_t start_position = vec3_t::Zero();
start_position[axis] = FT(1.0);
const vec3_t transformed = pose.Transform(start_position);
const vec3_t inverted = pose_inverse.Transform(transformed);
EXPECT_VEC3_NEAR(start_position, inverted, tolerance);
}
Pose_t nullified_pose[2] = {
pose.Compose(pose_inverse), pose_inverse.Compose(pose),
};
for (int i = 0; i < 2; ++i) {
EXPECT_QUAT_NEAR(quat_t::Identity(), nullified_pose[i].GetRotation(),
tolerance);
EXPECT_VEC3_NEAR(vec3_t::Zero(), nullified_pose[i].GetPosition(),
tolerance);
}
}
|
