Debian Codegraph @

libs/tfest/src/se3_unittest.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include <gtest/gtest.h>
11 #include <mrpt/math/ops_vectors.h>
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/poses/CPose3DQuat.h>
14 #include <mrpt/random.h>
15 #include <mrpt/tfest.h>
16
17 using namespace mrpt;
18 using namespace mrpt::tfest;
19 using namespace mrpt::math;
20 using namespace mrpt::random;
21 using namespace mrpt::poses;
22 using namespace std;
23
24 using TPoints = std::vector<std::vector<double>>;
25
26 // ------------------------------------------------------
27 // Generate both sets of points
28 // ------------------------------------------------------
29 CPose3DQuat generate_points(TPoints& pA, TPoints& pB)
30 {
31 const double Dx = 0.5;
32 const double Dy = 1.5;
33 const double Dz = 0.75;
34
35 const double yaw = 10.0_deg;
36 const double pitch = 20.0_deg;
37 const double roll = 5.0_deg;
38
39 pA.resize(5); // A set of points at "A" reference system
40 pB.resize(5); // A set of points at "B" reference system
41
42 pA[0].resize(3);
43 pA[0][0] = 0.0;
44 pA[0][1] = 0.5;
45 pA[0][2] = 0.4;
46 pA[1].resize(3);
47 pA[1][0] = 1.0;
48 pA[1][1] = 1.5;
49 pA[1][2] = -0.1;
50 pA[2].resize(3);
51 pA[2][0] = 1.2;
52 pA[2][1] = 1.1;
53 pA[2][2] = 0.9;
54 pA[3].resize(3);
55 pA[3][0] = 0.7;
56 pA[3][1] = 0.3;
57 pA[3][2] = 3.4;
58 pA[4].resize(3);
59 pA[4][0] = 1.9;
60 pA[4][1] = 2.5;
61 pA[4][2] = -1.7;
62
63 CPose3DQuat qPose = CPose3DQuat(CPose3D(Dx, Dy, Dz, yaw, pitch, roll));
64 for (unsigned int i = 0; i < 5; ++i)
65 {
66 pB[i].resize(3);
67 qPose.inverseComposePoint(pA[i][0], pA[i][1], pA[i][2], pB[i][0], pB[i][1], pB[i][2]);
68 }
69
70 return qPose;
71
72 } // end generate_points
73
74 // ------------------------------------------------------
75 // Generate a list of matched points
76 // ------------------------------------------------------
77 template <typename T>
78 void generate_list_of_points(const TPoints& pA, const TPoints& pB, TMatchingPairListTempl<T>& list)
79 {
80 TMatchingPairTempl<T> pair;
81 for (unsigned int i = 0; i < 5; ++i)
82 {
83 pair.globalIdx = pair.localIdx = i;
84 pair.global.x = d2f(pA[i][0]);
85 pair.global.y = d2f(pA[i][1]);
86 pair.global.z = d2f(pA[i][2]);
87
88 pair.local.x = d2f(pB[i][0]);
89 pair.local.y = d2f(pB[i][1]);
90 pair.local.z = d2f(pB[i][2]);
91
92 list.push_back(pair);
93 }
94 } // end generate_list_of_points
95
96 // ------------------------------------------------------
97 // Genreate a vector of matched points
98 // ------------------------------------------------------
99 void generate_vector_of_points(
100 const TPoints& pA,
101 const TPoints& pB,
102 vector<mrpt::math::TPoint3D>& ptsA,
103 vector<mrpt::math::TPoint3D>& ptsB)
104 {
105 // The input vector: inV = [pA1x, pA1y, pA1z, pB1x, pB1y, pB1z, ... ]
106 ptsA.resize(pA.size());
107 ptsB.resize(pA.size());
108 for (unsigned int i = 0; i < pA.size(); ++i)
109 {
110 ptsA[i] = mrpt::math::TPoint3D(pA[i][0], pA[i][1], pA[i][2]);
111 ptsB[i] = mrpt::math::TPoint3D(pB[i][0], pB[i][1], pB[i][2]);
112 }
113 } // end generate_vector_of_points
114
115 template <typename T>
116 void se3_l2_MatchList_test()
117 {
118 TPoints pA, pB; // The input points
119 CPose3DQuat qPose = generate_points(pA, pB);
120
121 TMatchingPairListTempl<T> list;
122 generate_list_of_points(pA, pB, list); // Generate a list of matched points
123
124 CPose3DQuat outQuat; // Output CPose3DQuat for the LSRigidTransformation
125 double scale; // Output scale value
126
127 bool res = mrpt::tfest::se3_l2(list, outQuat, scale);
128 EXPECT_TRUE(res);
129
130 double err = 0.0;
131 if ((qPose[3] * outQuat[3] > 0 && qPose[4] * outQuat[4] > 0 && qPose[5] * outQuat[5] > 0 &&
132 qPose[6] * outQuat[6] > 0) ||
133 (qPose[3] * outQuat[3] < 0 && qPose[4] * outQuat[4] < 0 && qPose[5] * outQuat[5] < 0 &&
134 qPose[6] * outQuat[6] < 0))
135 {
136 for (unsigned int i = 0; i < 7; ++i) err += square(std::fabs(qPose[i]) - std::fabs(outQuat[i]));
137 err = sqrt(err);
138 EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl
139 << qPose << endl
140 << "Out CPose3DQuat: " << endl
141 << outQuat << " [Err: " << err << "]" << endl;
142 }
143 else
144 {
145 GTEST_FAIL() << "Applied quaternion: " << endl
146 << qPose << endl
147 << "Out CPose3DQuat: " << endl
148 << outQuat << endl;
149 }
150 }
151
152 TEST(tfest, se3_l2_MatchList_float) { se3_l2_MatchList_test<float>(); }
153 TEST(tfest, se3_l2_MatchList_double) { se3_l2_MatchList_test<double>(); }
154
155 TEST(tfest, se3_l2_PtsLists)
156 {
157 TPoints pA, pB; // The input points
158 CPose3DQuat qPose = generate_points(pA, pB);
159
160 vector<mrpt::math::TPoint3D> ptsA, ptsB;
161 generate_vector_of_points(pA, pB, ptsA,
162 ptsB); // Generate a vector of matched points
163
164 mrpt::poses::CPose3DQuat qu;
165 double scale;
166 mrpt::tfest::se3_l2(ptsA, ptsB, qu,
167 scale); // Output quaternion for the Horn Method
168
169 double err = 0.0;
170 if ((qPose[3] * qu[3] > 0 && qPose[4] * qu[4] > 0 && qPose[5] * qu[5] > 0 &&
171 qPose[6] * qu[6] > 0) ||
172 (qPose[3] * qu[3] < 0 && qPose[4] * qu[4] < 0 && qPose[5] * qu[5] < 0 &&
173 qPose[6] * qu[6] < 0))
174 {
175 for (unsigned int i = 0; i < 7; ++i) err += square(std::fabs(qPose[i]) - std::fabs(qu[i]));
176 err = sqrt(err);
177 EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl
178 << qPose << endl
179 << "Out CPose3DQuat: " << endl
180 << qu << " [Err: " << err << "]" << endl;
181 }
182 else
183 {
184 GTEST_FAIL() << "Applied quaternion: " << endl
185 << qPose << endl
186 << "Out CPose3DQuat: " << endl
187 << qu << endl;
188 }
189 } // end
190
191 TEST(tfest, se3_l2_robust)
192 {
193 TPoints pA, pB; // The input points
194 CPose3DQuat qPose = generate_points(pA, pB);
195
196 TMatchingPairList list;
197 generate_list_of_points(pA, pB, list); // Generate a list of matched points
198
199 mrpt::tfest::TSE3RobustResult estim_result;
200 mrpt::tfest::TSE3RobustParams params;
201 params.ransac_minSetSize = 3;
202 params.ransac_maxSetSizePct = 3.0 / list.size();
203 mrpt::tfest::se3_l2_robust(list, params, estim_result);
204
205 EXPECT_GT(estim_result.inliers_idx.size(), 0u);
206 const CPose3DQuat& outQuat = estim_result.transformation;
207 double err = 0.0;
208 if ((qPose[3] * outQuat[3] > 0 && qPose[4] * outQuat[4] > 0 && qPose[5] * outQuat[5] > 0 &&
209 qPose[6] * outQuat[6] > 0) ||
210 (qPose[3] * outQuat[3] < 0 && qPose[4] * outQuat[4] < 0 && qPose[5] * outQuat[5] < 0 &&
211 qPose[6] * outQuat[6] < 0))
212 {
213 for (unsigned int i = 0; i < 7; ++i) err += square(std::fabs(qPose[i]) - std::fabs(outQuat[i]));
214 err = sqrt(err);
215 EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl
216 << qPose << endl
217 << "Out CPose3DQuat: " << endl
218 << outQuat << " [Err: " << err << "]" << endl;
219 }
220 else
221 {
222 GTEST_FAIL() << "Applied quaternion: " << endl
223 << qPose << endl
224 << "Out CPose3DQuat: " << endl
225 << outQuat << endl;
226 }
227 }