Skip to content

Commit 92f430c

Browse files
jorgenfjjorgenfj
authored
pose from eigen function (#25)
* pose from eigen function * update pose::from_eigen to aggregate init * removed eta and nu var names from tests
1 parent 6088963 commit 92f430c

File tree

2 files changed

+95
-52
lines changed

2 files changed

+95
-52
lines changed

‎cpp_test/test_types.cpp‎

Lines changed: 77 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -9,41 +9,41 @@ class TypesTests : public ::testing::Test {
99
void SetUp() override {}
1010
};
1111

12-
TEST_F(TypesTests, test_eta) {
13-
vortex::utils::types::PoseEuler eta;
12+
TEST_F(TypesTests, test_pose) {
13+
vortex::utils::types::PoseEuler pose;
1414
// Test correct zero initialization
15-
EXPECT_EQ(eta.x, 0.0);
16-
EXPECT_EQ(eta.y, 0.0);
17-
EXPECT_EQ(eta.z, 0.0);
18-
EXPECT_EQ(eta.roll, 0.0);
19-
EXPECT_EQ(eta.pitch, 0.0);
20-
EXPECT_EQ(eta.yaw, 0.0);
15+
EXPECT_EQ(pose.x, 0.0);
16+
EXPECT_EQ(pose.y, 0.0);
17+
EXPECT_EQ(pose.z, 0.0);
18+
EXPECT_EQ(pose.roll, 0.0);
19+
EXPECT_EQ(pose.pitch, 0.0);
20+
EXPECT_EQ(pose.yaw, 0.0);
2121

2222
// Test rotation and transformation matrix
23-
eta.roll = 1.0;
24-
eta.pitch = 0.5;
25-
eta.yaw = 1.7;
23+
pose.roll = 1.0;
24+
pose.pitch = 0.5;
25+
pose.yaw = 1.7;
2626
Eigen::Matrix3d expected_rm{
2727
vortex::utils::math::get_rotation_matrix(1.0, 0.5, 1.7)};
28-
Eigen::Matrix3d result_rm{eta.as_rotation_matrix()};
28+
Eigen::Matrix3d result_rm{pose.as_rotation_matrix()};
2929
EXPECT_TRUE(result_rm.isApprox(expected_rm, 1e-12));
3030

3131
Eigen::Matrix3d expected_tm{
3232
vortex::utils::math::get_transformation_matrix_attitude(1.0, 0.5)};
33-
Eigen::Matrix3d result_tm{eta.as_transformation_matrix()};
33+
Eigen::Matrix3d result_tm{pose.as_transformation_matrix()};
3434
EXPECT_TRUE(result_tm.isApprox(expected_tm, 1e-12));
3535

3636
// Test to_vector
37-
eta.x = 5.0;
38-
eta.y = -4.0;
39-
eta.z = 2.1;
40-
Eigen::Vector<double, 6> result_v{eta.to_vector()};
37+
pose.x = 5.0;
38+
pose.y = -4.0;
39+
pose.z = 2.1;
40+
Eigen::Vector<double, 6> result_v{pose.to_vector()};
4141
Eigen::Vector<double, 6> expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7};
4242
EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12));
4343

4444
// Test operator-
4545
vortex::utils::types::PoseEuler other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3};
46-
vortex::utils::types::PoseEuler diff{eta - other};
46+
vortex::utils::types::PoseEuler diff{pose - other};
4747
EXPECT_NEAR(diff.x, 4.0, 1e-12);
4848
EXPECT_NEAR(diff.y, -6.0, 1e-12);
4949
EXPECT_NEAR(diff.z, -0.9, 1e-12);
@@ -52,32 +52,32 @@ TEST_F(TypesTests, test_eta) {
5252
EXPECT_NEAR(diff.yaw, 1.4, 1e-12);
5353
}
5454

55-
TEST_F(TypesTests, test_eta_quat) {
56-
vortex::utils::types::Pose eta_quat;
55+
TEST_F(TypesTests, test_pose_quat) {
56+
vortex::utils::types::Pose pose_quat;
5757
// Test correct zero initialization
58-
EXPECT_EQ(eta_quat.x, 0.0);
59-
EXPECT_EQ(eta_quat.y, 0.0);
60-
EXPECT_EQ(eta_quat.z, 0.0);
61-
EXPECT_EQ(eta_quat.qw, 1.0);
62-
EXPECT_EQ(eta_quat.qx, 0.0);
63-
EXPECT_EQ(eta_quat.qy, 0.0);
64-
EXPECT_EQ(eta_quat.qz, 0.0);
58+
EXPECT_EQ(pose_quat.x, 0.0);
59+
EXPECT_EQ(pose_quat.y, 0.0);
60+
EXPECT_EQ(pose_quat.z, 0.0);
61+
EXPECT_EQ(pose_quat.qw, 1.0);
62+
EXPECT_EQ(pose_quat.qx, 0.0);
63+
EXPECT_EQ(pose_quat.qy, 0.0);
64+
EXPECT_EQ(pose_quat.qz, 0.0);
6565

6666
// Test to_vector
67-
eta_quat.x = 5.0;
68-
eta_quat.y = -4.0;
69-
eta_quat.z = 2.1;
70-
eta_quat.qw = 1.0;
71-
eta_quat.qx = 0.5;
72-
eta_quat.qy = -0.5;
73-
eta_quat.qz = 0.25;
74-
Eigen::Vector<double, 7> result_v{eta_quat.to_vector()};
67+
pose_quat.x = 5.0;
68+
pose_quat.y = -4.0;
69+
pose_quat.z = 2.1;
70+
pose_quat.qw = 1.0;
71+
pose_quat.qx = 0.5;
72+
pose_quat.qy = -0.5;
73+
pose_quat.qz = 0.25;
74+
Eigen::Vector<double, 7> result_v{pose_quat.to_vector()};
7575
Eigen::Vector<double, 7> expected_v{5.0, -4.0, 2.1, 1.0, 0.5, -0.5, 0.25};
7676
EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12));
7777

7878
// Test operator-
7979
vortex::utils::types::Pose other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.4};
80-
vortex::utils::types::Pose diff{eta_quat - other};
80+
vortex::utils::types::Pose diff{pose_quat - other};
8181
auto pos = diff.pos_vector();
8282
EXPECT_TRUE(pos.isApprox(Eigen::Vector3d(4.0, -6.0, -0.9), 1e-12));
8383
auto q = diff.ori_quaternion();
@@ -87,30 +87,55 @@ TEST_F(TypesTests, test_eta_quat) {
8787
1e-12));
8888
}
8989

90-
TEST_F(TypesTests, test_nu) {
91-
vortex::utils::types::Twist nu;
90+
TEST_F(TypesTests, test_pose_from_eigen) {
91+
using vortex::utils::types::Pose;
92+
93+
Eigen::Vector3d pos(1.0, -2.0, 3.5);
94+
95+
// Deliberately NOT normalized
96+
Eigen::Quaterniond ori(2.0, -1.0, 0.5, 0.25);
97+
98+
Pose pose = Pose::from_eigen(pos, ori);
99+
100+
// --- Position mapping ---
101+
EXPECT_DOUBLE_EQ(pose.x, pos.x());
102+
EXPECT_DOUBLE_EQ(pose.y, pos.y());
103+
EXPECT_DOUBLE_EQ(pose.z, pos.z());
104+
105+
// --- Orientation normalization ---
106+
Eigen::Quaterniond expected_q = ori.normalized();
107+
Eigen::Quaterniond result_q(pose.qw, pose.qx, pose.qy, pose.qz);
108+
109+
EXPECT_TRUE(result_q.isApprox(expected_q, 1e-12));
110+
111+
// --- Quaternion must be unit length ---
112+
EXPECT_NEAR(result_q.norm(), 1.0, 1e-12);
113+
}
114+
115+
TEST_F(TypesTests, test_twist) {
116+
vortex::utils::types::Twist twist;
92117
// Test correct zero initialization
93-
EXPECT_EQ(nu.u, 0.0);
94-
EXPECT_EQ(nu.v, 0.0);
95-
EXPECT_EQ(nu.w, 0.0);
96-
EXPECT_EQ(nu.p, 0.0);
97-
EXPECT_EQ(nu.q, 0.0);
98-
EXPECT_EQ(nu.r, 0.0);
118+
EXPECT_EQ(twist.u, 0.0);
119+
EXPECT_EQ(twist.v, 0.0);
120+
EXPECT_EQ(twist.w, 0.0);
121+
EXPECT_EQ(twist.p, 0.0);
122+
EXPECT_EQ(twist.q, 0.0);
123+
EXPECT_EQ(twist.r, 0.0);
99124

100125
// Test to_vector
101-
nu.u = 5.0;
102-
nu.v = -4.0;
103-
nu.w = 2.1;
104-
nu.p = 1.0;
105-
nu.q = 0.5;
106-
nu.r = 1.7;
107-
Eigen::Vector<double, 6> result_v{nu.to_vector()};
126+
twist.u = 5.0;
127+
twist.v = -4.0;
128+
twist.w = 2.1;
129+
twist.p = 1.0;
130+
twist.q = 0.5;
131+
twist.r = 1.7;
132+
Eigen::Vector<double, 6> result_v{twist.to_vector()};
108133
Eigen::Vector<double, 6> expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7};
109134
EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12));
110135

111136
// Test operator-
112137
vortex::utils::types::Twist other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3};
113-
vortex::utils::types::Twist diff{nu - other};
138+
vortex::utils::types::Twist diff{twist - other};
114139
EXPECT_NEAR(diff.u, 4.0, 1e-12);
115140
EXPECT_NEAR(diff.v, -6.0, 1e-12);
116141
EXPECT_NEAR(diff.w, -0.9, 1e-12);

‎include/vortex/utils/types.hpp‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,24 @@ struct Pose {
146146
double qy{};
147147
double qz{};
148148

149+
/**
150+
* @brief Construct a Pose from eigen components.
151+
* @param pos Eigen::Vector3d position component
152+
* @param ori Eigen::Quaterniond orientation component
153+
* @return Pose with normalized quaternion
154+
*/
155+
static Pose from_eigen(const Eigen::Vector3d& pos,
156+
const Eigen::Quaterniond& ori) {
157+
const Eigen::Quaterniond q = ori.normalized();
158+
return Pose{.x = pos.x(),
159+
.y = pos.y(),
160+
.z = pos.z(),
161+
.qw = q.w(),
162+
.qx = q.x(),
163+
.qy = q.y(),
164+
.qz = q.z()};
165+
}
166+
149167
/**
150168
* @brief Get the position vector (x, y, z).
151169
* @return Eigen::Vector3d{x, y, z}

0 commit comments

Comments
 (0)