Debian Codegraph @

libs/tfest/src/se3_l2.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 "tfest-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/poses/CPose3DQuat.h>
14 #include <mrpt/tfest/se3.h>
15
16 #include <Eigen/Dense>
17
18 using namespace mrpt;
19 using namespace mrpt::tfest;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 using namespace std;
23
24 // "Closed-form solution of absolute orientation using unit quaternions", BKP
25 // Horn, Journal of the Optical Society of America, 1987.
26 // Algorithm:
27 // 0. Preliminary
28 // pLi = { pLix, pLiy, pLiz }
29 // pRi = { pRix, pRiy, pRiz }
30 // -------------------------------------------------------
31 // 1. Find the centroids of the two sets of measurements:
32 // ct_others = (1/n)*sum{i}( pLi ) ct_others = { cLx, cLy, cLz }
33 // ct_this = (1/n)*sum{i}( pRi ) ct_this = { cRx, cRy, cRz }
34 //
35 // 2. Substract centroids from the point coordinates:
36 // pLi' = pLi - ct_others pLi' = { pLix', pLiy', pLiz' }
37 // pRi' = pRi - ct_this pRi' = { pRix', pRiy', pRiz' }
38 //
39 // 3. For each pair of coordinates (correspondences) compute the nine possible
40 // products:
41 // pi1 = pLix'*pRix' pi2 = pLix'*pRiy' pi3 = pLix'*pRiz'
42 // pi4 = pLiy'*pRix' pi5 = pLiy'*pRiy' pi6 = pLiy'*pRiz'
43 // pi7 = pLiz'*pRix' pi8 = pLiz'*pRiy' pi9 = pLiz'*pRiz'
44 //
45 // 4. Compute S components:
46 // Sxx = sum{i}( pi1 ) Sxy = sum{i}( pi2 ) Sxz = sum{i}( pi3 )
47 // Syx = sum{i}( pi4 ) Syy = sum{i}( pi5 ) Syz = sum{i}( pi6 )
48 // Szx = sum{i}( pi7 ) Szy = sum{i}( pi8 ) Szz = sum{i}( pi9 )
49 //
50 // 5. Compute N components:
51 // [ Sxx+Syy+Szz Syz-Szy Szx-Sxz Sxy-Syx ]
52 // [ Syz-Szy Sxx-Syy-Szz Sxy+Syx Szx+Sxz ]
53 // N = [ Szx-Sxz Sxy+Syx -Sxx+Syy-Szz Syz+Szy ]
54 // [ Sxy-Syx Szx+Sxz Syz+Szy -Sxx-Syy+Szz ]
55 //
56 // 6. Rotation represented by the quaternion eigenvector correspondent to the
57 // higher eigenvalue of N
58 //
59 // 7. Scale computation (symmetric expression)
60 // s = sqrt( sum{i}( square(std::abs(pRi')) / sum{i}(
61 // square(std::abs(pLi')) ) )
62 //
63 // 8. Translation computation (distance between the Right centroid and the
64 // scaled and rotated Left centroid)
65 // t = ct_this-sR(ct_others)
66
67 /*---------------------------------------------------------------
68 se3_l2 (old "HornMethod()")
69 ---------------------------------------------------------------*/
70 namespace
71 {
72 bool se3_l2_internal(
73 std::vector<mrpt::math::TPoint3D>& points_this, // IN/OUT: It gets modified!
74 std::vector<mrpt::math::TPoint3D>& points_other, // IN/OUT: It gets modified!
75 mrpt::poses::CPose3DQuat& out_transform,
76 double& out_scale,
77 bool forceScaleToUnity)
78 {
79 MRPT_START
80
81 ASSERT_EQUAL_(points_this.size(), points_other.size());
82 // Compute the centroids
83 TPoint3D ct_others(0, 0, 0), ct_this(0, 0, 0);
84 const size_t nMatches = points_this.size();
85
86 if (nMatches < 3) return false; // Nothing we can estimate without 3 points!!
87
88 for (size_t i = 0; i < nMatches; i++)
89 {
90 ct_others += points_other[i];
91 ct_this += points_this[i];
92 }
93
94 const double F = 1.0 / nMatches;
95 ct_others *= F;
96 ct_this *= F;
97
98 CMatrixDouble33 S; // Zeroed by default
99
100 // Substract the centroid and compute the S matrix of cross products
101 for (size_t i = 0; i < nMatches; i++)
102 {
103 points_this[i] -= ct_this;
104 points_other[i] -= ct_others;
105
106 S(0, 0) += points_other[i].x * points_this[i].x;
107 S(0, 1) += points_other[i].x * points_this[i].y;
108 S(0, 2) += points_other[i].x * points_this[i].z;
109
110 S(1, 0) += points_other[i].y * points_this[i].x;
111 S(1, 1) += points_other[i].y * points_this[i].y;
112 S(1, 2) += points_other[i].y * points_this[i].z;
113
114 S(2, 0) += points_other[i].z * points_this[i].x;
115 S(2, 1) += points_other[i].z * points_this[i].y;
116 S(2, 2) += points_other[i].z * points_this[i].z;
117 }
118
119 // Construct the N matrix
120 CMatrixDouble44 N; // Zeroed by default
121
122 N(0, 0) = S(0, 0) + S(1, 1) + S(2, 2);
123 N(0, 1) = S(1, 2) - S(2, 1);
124 N(0, 2) = S(2, 0) - S(0, 2);
125 N(0, 3) = S(0, 1) - S(1, 0);
126
127 N(1, 0) = N(0, 1);
128 N(1, 1) = S(0, 0) - S(1, 1) - S(2, 2);
129 N(1, 2) = S(0, 1) + S(1, 0);
130 N(1, 3) = S(2, 0) + S(0, 2);
131
132 N(2, 0) = N(0, 2);
133 N(2, 1) = N(1, 2);
134 N(2, 2) = -S(0, 0) + S(1, 1) - S(2, 2);
135 N(2, 3) = S(1, 2) + S(2, 1);
136
137 N(3, 0) = N(0, 3);
138 N(3, 1) = N(1, 3);
139 N(3, 2) = N(2, 3);
140 N(3, 3) = -S(0, 0) - S(1, 1) + S(2, 2);
141
142 // q is the quaternion correspondent to the greatest eigenvector of the N
143 // matrix (last column in Z)
144 CMatrixDouble44 Z;
145 vector<double> eigvals;
146 N.eig_symmetric(Z, eigvals, true /*sorted*/);
147
148 auto v = CVectorFixedDouble<4>(Z.col(3).eval());
149
150 ASSERTDEB_(fabs(sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]) - 1.0) < 0.1);
151
152 // Make q_r > 0
153 if (v[0] < 0)
154 {
155 v[0] *= -1;
156 v[1] *= -1;
157 v[2] *= -1;
158 v[3] *= -1;
159 }
160
161 // out_transform: Create a pose rotation with the quaternion
162 for (unsigned int i = 0; i < 4; i++) // insert the quaternion part
163 out_transform[i + 3] = v[i];
164
165 // Compute scale
166 double s;
167 if (forceScaleToUnity)
168 {
169 s = 1.0; // Enforce scale to be 1
170 }
171 else
172 {
173 double num = 0.0;
174 double den = 0.0;
175 for (size_t i = 0; i < nMatches; i++)
176 {
177 num += square(points_other[i].x) + square(points_other[i].y) + square(points_other[i].z);
178 den += square(points_this[i].x) + square(points_this[i].y) + square(points_this[i].z);
179 } // end-for
180
181 // The scale:
182 s = std::sqrt(num / den);
183 }
184
185 TPoint3D pp;
186 out_transform.composePoint(ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z);
187 pp *= s;
188
189 out_transform[0] = ct_this.x - pp.x; // X
190 out_transform[1] = ct_this.y - pp.y; // Y
191 out_transform[2] = ct_this.z - pp.z; // Z
192
193 out_scale = s; // return scale
194 return true;
195
196 MRPT_END
197 } // end se3_l2_internal()
198 } // namespace
199
200 bool tfest::se3_l2(
201 const std::vector<mrpt::math::TPoint3D>& in_points_this,
202 const std::vector<mrpt::math::TPoint3D>& in_points_other,
203 mrpt::poses::CPose3DQuat& out_transform,
204 double& out_scale,
205 bool forceScaleToUnity)
206 {
207 // make a copy because we need it anyway to substract the centroid and to
208 // provide a unified interface to TMatchingList API
209 std::vector<mrpt::math::TPoint3D> points_this = in_points_this;
210 std::vector<mrpt::math::TPoint3D> points_other = in_points_other;
211
212 return se3_l2_internal(points_this, points_other, out_transform, out_scale, forceScaleToUnity);
213 }
214
215 bool tfest::se3_l2(
216 const mrpt::tfest::TMatchingPairList& corrs,
217 mrpt::poses::CPose3DQuat& out_transform,
218 double& out_scale,
219 bool forceScaleToUnity)
220 {
221 // Transform data types:
222 const size_t N = corrs.size();
223 std::vector<mrpt::math::TPoint3D> points_this(N), points_other(N);
224 for (size_t i = 0; i < N; i++)
225 {
226 points_this[i].x = corrs[i].global.x;
227 points_this[i].y = corrs[i].global.y;
228 points_this[i].z = corrs[i].global.z;
229 points_other[i].x = corrs[i].local.x;
230 points_other[i].y = corrs[i].local.y;
231 points_other[i].z = corrs[i].local.z;
232 }
233 return se3_l2_internal(points_this, points_other, out_transform, out_scale, forceScaleToUnity);
234 }
235
236 bool tfest::se3_l2(
237 const mrpt::tfest::TMatchingPairList_d& corrs,
238 mrpt::poses::CPose3DQuat& out_transform,
239 double& out_scale,
240 bool forceScaleToUnity)
241 {
242 // Transform data types:
243 const size_t N = corrs.size();
244 std::vector<mrpt::math::TPoint3D> points_this(N), points_other(N);
245 for (size_t i = 0; i < N; i++)
246 {
247 points_this[i].x = corrs[i].global.x;
248 points_this[i].y = corrs[i].global.y;
249 points_this[i].z = corrs[i].global.z;
250 points_other[i].x = corrs[i].local.x;
251 points_other[i].y = corrs[i].local.y;
252 points_other[i].z = corrs[i].local.z;
253 }
254 return se3_l2_internal(points_this, points_other, out_transform, out_scale, forceScaleToUnity);
255 }