-
Notifications
You must be signed in to change notification settings - Fork 25
Expand file tree
/
Copy pathpose_vector_test.cpp
More file actions
150 lines (115 loc) · 3.66 KB
/
pose_vector_test.cpp
File metadata and controls
150 lines (115 loc) · 3.66 KB
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
/*
* This is part of the Bayesian Object Tracking (bot),
* (https://github.com/bayesian-object-tracking)
*
* Copyright (c) 2015 Max Planck Society,
* Autonomous Motion Department,
* Institute for Intelligent Systems
*
* This Source Code Form is subject to the terms of the GNU General Public
* License License (GNU GPL). A copy of the license can be found in the LICENSE
* file distributed with this source code.
*/
/**
* \date 2015
* \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
*/
#include <Eigen/Core>
#include <dbot/pose/pose_vector.h>
#include <gtest/gtest.h>
using namespace dbot;
typedef double Real;
typedef EulerVector::RotationMatrix RotationMatrix;
typedef EulerVector::AngleAxis AngleAxis;
typedef EulerVector::Quaternion Quaternion;
typedef Eigen::Matrix<Real, 4, 4> HomogeneousMatrix;
typedef Eigen::Matrix<Real, 6, 1> Vector6d;
typedef Eigen::Matrix<Real, 3, 1> Vector3d;
typedef Eigen::Matrix<Real, 4, 1> Vector4d;
typedef PoseVector::Affine Affine;
Real epsilon = 0.000000001;
TEST(pose_vector, equality)
{
Vector6d vector = Vector6d::Random();
PoseVector pose_vector = vector;
EXPECT_TRUE(pose_vector.isApprox(vector));
}
TEST(pose_vector, position)
{
Vector3d vector = Vector3d::Random();
PoseVector pose_vector;
pose_vector.position() = vector;
EXPECT_TRUE(pose_vector.position().isApprox(vector));
}
TEST(pose_vector, euler_vector)
{
Vector3d vector = Vector3d::Random();
PoseVector pose_vector;
pose_vector.orientation() = vector;
EXPECT_TRUE(pose_vector.orientation().isApprox(vector));
}
TEST(pose_vector, quaternion)
{
EulerVector euler = EulerVector::Random();
PoseVector pose_vector;
pose_vector.orientation().quaternion(euler.quaternion());
EXPECT_TRUE(pose_vector.orientation().isApprox(euler));
}
TEST(pose_vector, get_homogeneous)
{
PoseVector pose_vector = PoseVector::Random();
Vector3d va = Vector3d::Random();
Vector4d vb;
vb.topRows(3) = va;
vb(3) = 1;
va = pose_vector.orientation().rotation_matrix() * va +
pose_vector.position();
vb = pose_vector.homogeneous() * vb;
EXPECT_TRUE(va.isApprox(vb.topRows(3)));
}
TEST(pose_vector, set_homogeneous)
{
PoseVector pose_vector1 = PoseVector::Random();
PoseVector pose_vector2;
pose_vector2.homogeneous(pose_vector1.homogeneous());
EXPECT_TRUE(pose_vector1.isApprox(pose_vector2));
}
TEST(pose_vector, get_affine)
{
PoseVector pose_vector = PoseVector::Random();
Vector3d va = Vector3d::Random();
Vector3d vb = va;
pose_vector.position() = Vector3d::Zero();
va = pose_vector.orientation().rotation_matrix() * va +
pose_vector.position();
vb = pose_vector.affine() * vb;
EXPECT_TRUE(va.isApprox(vb));
}
TEST(pose_vector, set_affine)
{
PoseVector pose_vector1 = PoseVector::Random();
PoseVector pose_vector2;
pose_vector2.affine(pose_vector1.affine());
EXPECT_TRUE(pose_vector1.isApprox(pose_vector2));
}
TEST(pose_vector, product)
{
PoseVector v1 = PoseVector::Random();
PoseVector v2 = PoseVector::Random();
PoseVector correct_result;
correct_result.orientation().rotation_matrix(
v2.orientation().rotation_matrix() *
v1.orientation().rotation_matrix());
correct_result.position() =
v2.orientation().rotation_matrix() * v1.position() + v2.position();
PoseVector operator_result = v2 * v1;
EXPECT_TRUE(correct_result.isApprox(operator_result));
}
TEST(pose_vector, inverse)
{
PoseVector v = PoseVector::Random();
PoseVector result = v * v.inverse();
EXPECT_TRUE(result.norm() < 0.00001);
result = v.inverse() * v;
EXPECT_TRUE(result.norm() < 0.00001);
}