10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
16 #include <Eigen/Dense>
23 template class mrpt::CTraitsTest<CPose3DQuat>;
31 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
32 double roll1,
double x2,
double y2,
double z2,
double yaw2,
33 double pitch2,
double roll2)
35 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
36 const CPose3D p2(x2, y2, z2, yaw2, pitch2, roll2);
38 const CPose3D p1_c_p2 = p1 + p2;
39 const CPose3D p1_i_p2 = p1 - p2;
57 <<
"p1_c_p2: " << p1_c_p2 << endl
58 <<
"q1_c_p2: " << p_q1_c_q2 << endl;
67 <<
"p1_i_p2: " << p1_i_p2 << endl
68 <<
"q1_i_p2: " << p_q1_i_q2 << endl;
76 (
A.asVectorVal() - q1_c_q2.
asVectorVal()).array().abs().sum(),
85 (
A.asVectorVal() - q1_c_q2.
asVectorVal()).array().abs().sum(),
94 (
A.asVectorVal() - q1_c_q2.
asVectorVal()).array().abs().sum(),
100 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
101 double roll1,
double x,
double y,
double z)
103 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
117 <<
"p1: " << p1 << endl
118 <<
"q1: " << q1 << endl
119 <<
"p: " << p << endl
120 <<
"p1_plus_p: " << p1_plus_p << endl
121 <<
"q1_plus_p: " << q1_plus_p << endl;
131 const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
133 for (
int i = 0; i < 3; i++) Y[i] = pp[i];
137 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
138 double roll1,
double x,
double y,
double z)
154 for (
int i = 0; i < 7; i++) x_mean[i] = q1[i];
168 x_incrs, DUMMY, numJacobs);
170 num_df_dpose = numJacobs.
asEigen().block<3, 7>(0, 0);
171 num_df_dpoint = numJacobs.
asEigen().block<3, 3>(0, 7);
177 (df_dpoint.
asEigen() - num_df_dpoint.
asEigen()).array().abs().sum(),
179 <<
"q1: " << q1 << endl
180 <<
"p: " << p << endl
181 <<
"Numeric approximation of df_dpoint: " << endl
182 << num_df_dpoint.
asEigen() << endl
183 <<
"Implemented method: " << endl
190 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
192 <<
"q1: " << q1 << endl
193 <<
"p: " << p << endl
194 <<
"Numeric approximation of df_dpose: " << endl
195 << num_df_dpose.
asEigen() << endl
196 <<
"Implemented method: " << endl
203 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
204 double roll1,
double x,
double y,
double z)
206 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
217 (p_minus_p1.
asVectorVal() - p_minus_q1.asVectorVal())
222 <<
"p_minus_p1: " << p_minus_p1 << endl
223 <<
"p_minus_q1: " << p_minus_q1 << endl;
228 <<
"p_rec: " << p_rec << endl
229 <<
"p: " << p << endl;
239 const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
247 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
248 double roll1,
double x,
double y,
double z)
262 const double qr = q1.
quat().
r();
263 const double qx = q1.
quat().
x();
264 const double qy = q1.
quat().
y();
265 const double qz = q1.
quat().
z();
266 const double Ax = x - x1;
267 const double Ay = y - y1;
268 const double Az = z - z1;
269 theorical.
x = Ax + 2 * (Ay) * (qr * qz + qx * qy) -
270 2 * (Az) * (qr * qy - qx * qz) -
272 theorical.
y = Ay - 2 * (Ax) * (qr * qz - qx * qy) +
273 2 * (Az) * (qr * qx + qy * qz) -
275 theorical.
z = Az + 2 * (Ax) * (qr * qy + qx * qz) -
276 2 * (Ay) * (qr * qx - qy * qz) -
288 for (
int i = 0; i < 7; i++) x_mean[i] = q1[i];
302 x_incrs, DUMMY, numJacobs);
304 num_df_dpose = numJacobs.
block<3, 7>(0, 0);
305 num_df_dpoint = numJacobs.
block<3, 3>(0, 7);
311 (df_dpoint.
asEigen() - num_df_dpoint.
asEigen()).array().abs().sum(),
313 <<
"q1: " << q1 << endl
314 <<
"from pose: " <<
CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
315 <<
"p: " << p << endl
316 <<
"local: " << l << endl
317 <<
"Numeric approximation of df_dpoint: " << endl
318 << num_df_dpoint.
asEigen() << endl
319 <<
"Implemented method: " << endl
322 << df_dpoint - num_df_dpoint << endl;
326 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
328 <<
"q1: " << q1 << endl
329 <<
"from pose: " <<
CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
330 <<
"p: " << p << endl
331 <<
"local: " << l << endl
332 <<
"Numeric approximation of df_dpose: " << endl
333 << num_df_dpose.
asEigen() << endl
334 <<
"Implemented method: " << endl
341 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
344 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
356 <<
"p1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
358 <<
"q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
363 <<
"p1: " << p1 << endl
364 <<
"q1: " << q1 << endl
365 <<
"p1r: " << p1r << endl;
369 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
372 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
386 <<
"p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
388 <<
"q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
393 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
396 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
409 <<
"q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
411 <<
"q2.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
416 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
417 double roll1,
double x,
double y,
double z)
430 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
431 double roll1,
double x,
double y,
double z)
433 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
445 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
446 double roll1,
double x,
double y,
double z)
448 const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
461 double gx = x, gy = y, gz = z;
463 double dist, yaw,
pitch;
469 double lx2, ly2, lz2;
485 const TPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
490 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
491 double roll1,
double x,
double y,
double z)
507 for (
int i = 0; i < 7; i++) x_mean[i] = q1[i];
521 x_incrs, DUMMY, numJacobs);
523 num_df_dpose = numJacobs.
block<3, 7>(0, 0);
524 num_df_dpoint = numJacobs.
block<3, 3>(0, 7);
530 (df_dpoint.
asEigen() - num_df_dpoint.
asEigen()).array().abs().sum(),
532 <<
"q1: " << q1 << endl
533 <<
"p: " << p << endl
534 <<
"Numeric approximation of df_dpoint: " << endl
535 << num_df_dpoint.
asEigen() << endl
536 <<
"Implemented method: " << endl
543 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
545 <<
"q1: " << q1 << endl
546 <<
"p: " << p << endl
547 <<
"Numeric approximation of df_dpose: " << endl
548 << num_df_dpose.
asEigen() << endl
549 <<
"Implemented method: " << endl
560 for (
int i = 0; i < 4; i++) q[i] = x[i];
562 for (
int i = 0; i < 4; i++) Y[i] = q[i];
567 const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
578 for (
int i = 0; i < 4; i++) x_mean[i] = q1[i];
589 x_incrs, DUMMY, numJacobs);
591 num_df_dpose = numJacobs.
block<4, 4>(0, 0);
597 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
599 <<
"q1: " << q1 << endl
600 <<
"Numeric approximation of df_dpose: " << endl
601 << num_df_dpose.
asEigen() << endl
602 <<
"Implemented method: " << endl
611 double yaw,
double pitch,
double roll,
double qx,
double qy,
double qz,
612 double qw,
const std::string& sRotMat)
614 const double eps = 1e-4;
619 std::stringstream sErr;
621 GTEST_FAIL() <<
"Incorrect R_gt matrix: '" << sRotMat
622 <<
"'. Error: " << sErr.str() <<
"\n";
623 EXPECT_EQ(R_gt.
cols(), 3) <<
" for: sRotMat='" << sRotMat <<
"'\n";
624 EXPECT_EQ(R_gt.
rows(), 3) <<
" for: sRotMat='" << sRotMat <<
"'\n";
634 << R_gt <<
"\np.R=\n"
642 if (std::abs(q.
w() - q_gt.w()) >
eps || std::abs(q.
x() - q_gt.x()) >
eps ||
643 std::abs(q.
y() - q_gt.y()) >
eps || std::abs(q.
z() - q_gt.z()) >
eps)
645 GTEST_FAIL() <<
"q = " << q.
asString()
646 <<
"\nExpected = " << q_gt.asString() <<
"\n";
669 0.0_deg, 0.0_deg, 0.0_deg,
671 "[ 1.0000000 0.0000000 0.0000000; "
672 " 0.0000000 1.0000000 0.0000000; "
673 " 0.0000000 0.0000000 1.0000000 ]");
675 90.0_deg, 0.0_deg, 0.0_deg,
676 0, 0, 0.7071068, 0.7071068,
677 "[ 0.0000000 -1.0000000 0.0000000;"
678 " 1.0000000 0.0000000 0.0000000;"
679 " 0.0000000 0.0000000 1.0000000 ]");
681 30.0_deg, 10.0_deg, 60.0_deg,
682 0.4615897, 0.2018243, 0.1811979, 0.8446119,
683 "[ 0.8528686 -0.1197639 0.5082046;"
684 " 0.4924039 0.5082046 -0.7065880;"
685 " -0.1736482 0.8528686 0.4924039 ]");
687 -10.0_deg, -20.0_deg, -30.0_deg,
688 -0.2685358, -0.1448781, -0.1276794, 0.9437144,
689 "[ 0.9254166 0.3187958 -0.2048741;"
690 " -0.1631759 0.8231729 0.5438381;"
691 " 0.3420202 -0.4698463 0.8137977 ]");
693 -179.9995949_deg, -90.0_deg, 0.0_deg,
694 0.7071068, 0, 0.7071068, -0.000005,
695 "[ 0.0000000 0.0000071 1.0000000;"
696 " -0.0000071 -1.0000000 0.0000071;"
697 " 1.0000000 -0.0000071 0.0000000 ]");
702 test_fromYPRAndBack(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
703 test_fromYPRAndBack(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
704 test_fromYPRAndBack(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
705 test_fromYPRAndBack(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
706 test_fromYPRAndBack(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
707 test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
708 test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
713 test_unaryInverse(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
714 test_unaryInverse(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
715 test_unaryInverse(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
716 test_unaryInverse(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
717 test_unaryInverse(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
718 test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
719 test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
724 test_copy(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
725 test_copy(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
726 test_copy(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
727 test_copy(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
733 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 2.0, -5.0, 8.0, 40.0_deg,
737 25.0, 2.0, 3.0, -30.0_deg, 90.0_deg, 0.0_deg, -10.0, 4.0, -8.0,
738 20.0_deg, 9.0_deg, 0.0_deg);
743 test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
744 test_composePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
745 test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
746 test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
748 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
750 1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
755 test_composePointJacob(
756 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
757 test_composePointJacob(
758 1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
759 test_composePointJacob(
760 1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
761 test_composePointJacob(
762 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
763 test_composePointJacob(
764 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
765 test_composePointJacob(
766 1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
771 test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
772 test_invComposePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
773 test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
774 test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
775 test_invComposePoint(
776 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
777 test_invComposePoint(
778 1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
783 test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
784 test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 1, 2, 3);
785 test_invComposePointJacob(
786 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
787 test_invComposePointJacob(
788 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
789 test_invComposePointJacob(
790 1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
791 test_invComposePointJacob(
792 1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
793 test_invComposePointJacob(
794 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
795 test_invComposePointJacob(
796 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
797 test_invComposePointJacob(
798 1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
803 test_composeAndInvComposePoint(
804 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
805 test_composeAndInvComposePoint(
806 1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
807 test_composeAndInvComposePoint(
808 1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
809 test_composeAndInvComposePoint(
810 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
811 test_composeAndInvComposePoint(
812 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
813 test_composeAndInvComposePoint(
814 1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
819 test_composePoint_vs_CPose3D(
820 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
821 test_composePoint_vs_CPose3D(
822 1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
823 test_composePoint_vs_CPose3D(
824 1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
825 test_composePoint_vs_CPose3D(
826 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
827 test_composePoint_vs_CPose3D(
828 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
829 test_composePoint_vs_CPose3D(
830 1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
835 test_invComposePoint_vs_CPose3D(
836 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
837 test_invComposePoint_vs_CPose3D(
838 1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
839 test_invComposePoint_vs_CPose3D(
840 1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
841 test_invComposePoint_vs_CPose3D(
842 1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
844 for (
size_t i = 0; i < 10; i++)
846 std::vector<double> v(9);
848 test_invComposePoint_vs_CPose3D(
849 v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
855 test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
856 test_sphericalCoords(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
857 test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
858 test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
859 test_sphericalCoords(
860 1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
861 test_sphericalCoords(
862 1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
867 test_normalizeJacob(0.0_deg, 0.0_deg, 0.0_deg);
868 test_normalizeJacob(10.0_deg, 0.0_deg, 0.0_deg);
869 test_normalizeJacob(0.0_deg, 10.0_deg, 0.0_deg);
870 test_normalizeJacob(0.0_deg, 0.0_deg, 10.0_deg);
871 test_normalizeJacob(-30.0_deg, 10.0_deg, 60.0_deg);
872 test_normalizeJacob(10.0_deg, -50.0_deg, -40.0_deg);