10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
16 #include <Eigen/Dense>
23 template class mrpt::CTraitsTest<mrpt::poses::CPose3DQuat>;
24 template class mrpt::CTraitsTest<mrpt::poses::CPose3D>;
25 template class mrpt::CTraitsTest<mrpt::math::CQuaternionDouble>;
35 CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
41 std::abs((
CPose3D(q1, 0, 0, 0).asVectorVal() -
42 CPose3D(q1r, 0, 0, 0).asVectorVal())
49 CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
60 <<
"ypr->quat->ypr failed with:" << endl
61 <<
" p1:" << p1 << endl
62 <<
" q1:" << q1 << endl
63 <<
" p2:" << p2 << endl;
73 <<
"pose constructor from quat failed with:" << endl
74 <<
" p1:" << p1 << endl
75 <<
" q1:" << q1 << endl
76 <<
" p3:" << p3 << endl;
81 const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
115 << (q1_ln - v) << endl;
124 CPose3D p1(0, 0, 0, 10.0_deg, 30.0_deg, -20.0_deg);
127 CPose3D p2(0, 0, 0, 30.0_deg, -20.0_deg, 10.0_deg);
139 <<
"q1 = " << q1 << endl
140 <<
"q1 as CPose3D = " <<
CPose3D(q1, 0, 0, 0) << endl
141 <<
"q2 = " << q2 << endl
142 <<
"q2 as CPose3D = " <<
CPose3D(q2, 0, 0, 0) << endl
143 <<
"q3 = q1 * q2 = " << q3 << endl
144 <<
"q3 as CPose3D = " <<
CPose3D(q3, 0, 0, 0) << endl
145 <<
"Should be equal to p3 = p1 (+) p2 = " << p3 << endl;
151 test_gimbalLock(20.0_deg, 90.0_deg, 0.0_deg);
152 test_gimbalLock(20.0_deg, -90.0_deg, 0.0_deg);
157 test_toYPRAndBack(20.0_deg, 30.0_deg, 40.0_deg);
158 test_toYPRAndBack(20.0_deg, 30.0_deg, 40.0_deg);
159 test_toYPRAndBack(30.0_deg, 90.0_deg, 0.0_deg);
160 test_toYPRAndBack(-30.0_deg, 90.0_deg, 0.0_deg);
161 test_toYPRAndBack(-30.0_deg, 88.0_deg, 60.0_deg);
162 test_toYPRAndBack(-30.0_deg, 10.0_deg, 60.0_deg);
167 const double list_test_YPR_angles_degrees[][3] = {
168 {0, 0, 0}, {-1, 0, 0}, {1, 0, 0}, {0, -1, 0},
169 {0, 1, 0}, {0, 0, -1}, {0, 0, 1}, {40, 0, 0},
170 {0, 40, 0}, {0, 0, 40}, {-40, 0, 0}, {0, -40, 0},
171 {0, 0, -40}, {-30, 20, 50}, {-30, 20, -50}, {30, -20, -50}};
173 for (
const auto& list_test_YPR_angles_degree : list_test_YPR_angles_degrees)
174 test_lnAndExpMatches(
175 DEG2RAD(list_test_YPR_angles_degree[0]),
176 DEG2RAD(list_test_YPR_angles_degree[1]),
177 DEG2RAD(list_test_YPR_angles_degree[2]));
182 const double list_test_XYZ[][3] = {
183 {0, 0, 0}, {-1, 0, 0}, {1, 0, 0},
184 {0, -1, 0}, {0, 1, 0}, {0, 0, -1},
185 {0, 0, 1}, {1e-3, 0, 0}, {0, 1e-3, 0},
186 {0, 0, 1e-3}, {-1e-3, 0, 0}, {0, -1e-3, 0},
187 {0, 0, -1e-3}, {-0.1, 0.2, 0.3}, {-0.1, -0.2, 0.3},
188 {-0.1, -0.2, -0.3}, {-0.1, 0.2, -0.3}, {0.1, 0.2, -0.3},
191 for (
const auto& i : list_test_XYZ) test_ExpAndLnMatches(i[0], i[1], i[2]);