Debian Codegraph @

src/ImathTest/testProcrustes.cpp

raw
1 //
2 // SPDX-License-Identifier: BSD-3-Clause
3 // Copyright Contributors to the OpenEXR Project.
4 //
5
6 #ifdef NDEBUG
7 # undef NDEBUG
8 #endif
9
10 #include <ImathEuler.h>
11 #include <ImathMatrixAlgo.h>
12 #include <ImathRandom.h>
13 #include <assert.h>
14 #include <cmath>
15 #include <iostream>
16 #include <limits>
17 #include <vector>
18
19 // Verify that if our transformation is already orthogonal, procrustes doesn't
20 // change that:
21 template <typename T>
22 void
23 testTranslationRotationMatrix (const IMATH_INTERNAL_NAMESPACE::M44d& mat)
24 {
25 std::cout << "Testing known translate/rotate matrix:\n " << mat;
26 typedef IMATH_INTERNAL_NAMESPACE::Vec3<T> Vec;
27
28 static IMATH_INTERNAL_NAMESPACE::Rand48 rand (2047);
29
30 size_t numPoints = 7;
31 std::vector<Vec> from;
32 from.reserve (numPoints);
33 std::vector<Vec> to;
34 to.reserve (numPoints);
35 for (size_t i = 0; i < numPoints; ++i)
36 {
37 IMATH_INTERNAL_NAMESPACE::V3d a (rand.nextf(), rand.nextf(), rand.nextf());
38 IMATH_INTERNAL_NAMESPACE::V3d b = a * mat;
39
40 from.push_back (Vec (a));
41 to.push_back (Vec (b));
42 }
43
44 std::vector<T> weights (numPoints, T (1));
45 const IMATH_INTERNAL_NAMESPACE::M44d m1 =
46 procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], numPoints);
47 const IMATH_INTERNAL_NAMESPACE::M44d m2 =
48 procrustesRotationAndTranslation (&from[0], &to[0], numPoints);
49
50 const T eps = sizeof (T) == 8 ? T(1e-8) : T(1e-4);
51 for (size_t i = 0; i < numPoints; ++i)
52 {
53 const IMATH_INTERNAL_NAMESPACE::V3d a = from[i];
54 const IMATH_INTERNAL_NAMESPACE::V3d b = to[i];
55 const IMATH_INTERNAL_NAMESPACE::V3d b1 = a * m1;
56 const IMATH_INTERNAL_NAMESPACE::V3d b2 = a * m2;
57
58 assert ((b - b1).length() < eps);
59 assert ((b - b2).length() < eps);
60 }
61 std::cout << " OK\n";
62 }
63
64 // Test that if we pass in a matrix that we know consists only of translates,
65 // rotates, and uniform scale that we get an exact match.
66 template <typename T>
67 void
68 testWithTranslateRotateAndScale (const IMATH_INTERNAL_NAMESPACE::M44d& m)
69 {
70 std::cout << "Testing with known translate/rotate/scale matrix\n" << m;
71 IMATH_INTERNAL_NAMESPACE::Rand48 rand (5376);
72
73 typedef IMATH_INTERNAL_NAMESPACE::Vec3<T> V3;
74 std::vector<V3> from;
75 std::vector<T> weights;
76
77 const float eps = 1e-4f;
78 std::cout << "numPoints: " << std::flush;
79 for (size_t numPoints = 1; numPoints < 10; ++numPoints)
80 {
81 from.push_back (V3(
82 static_cast<T>(rand.nextf()),
83 static_cast<T>(rand.nextf()),
84 static_cast<T>(rand.nextf())));
85 weights.push_back(static_cast<T>(rand.nextf()));
86 std::cout << from.size () << " ";
87
88 std::vector<V3> to;
89 for (size_t i = 0; i < from.size(); ++i)
90 to.push_back (from[i] * m);
91
92 // weighted:
93 IMATH_INTERNAL_NAMESPACE::M44d res =
94 IMATH_INTERNAL_NAMESPACE::procrustesRotationAndTranslation (&from[0],
95 &to[0],
96 &weights[0],
97 from.size(),
98 true);
99 for (size_t i = 0; i < from.size(); ++i)
100 assert ((from[i] * res - to[i]).length() < eps);
101
102 // unweighted:
103 res = IMATH_INTERNAL_NAMESPACE::procrustesRotationAndTranslation (&from[0],
104 &to[0],
105 from.size(),
106 true);
107 for (size_t i = 0; i < from.size(); ++i)
108 assert ((from[i] * res - to[i]).length() < eps);
109 }
110 std::cout << " OK\n";
111 }
112
113 template <typename T>
114 double
115 procrustesError (const IMATH_INTERNAL_NAMESPACE::Vec3<T>* from,
116 const IMATH_INTERNAL_NAMESPACE::Vec3<T>* to,
117 const T* weights,
118 const size_t n,
119 const IMATH_INTERNAL_NAMESPACE::M44d& xform)
120 {
121 double result = 0.0;
122 double residual = 0.0;
123 for (size_t i = 0; i < n; ++i)
124 {
125 IMATH_INTERNAL_NAMESPACE::V3d xformed = IMATH_INTERNAL_NAMESPACE::V3d (from[i]) * xform;
126 IMATH_INTERNAL_NAMESPACE::V3d diff = xformed - IMATH_INTERNAL_NAMESPACE::V3d (to[i]);
127 const double w = weights[i];
128 const double mag = w * diff.length2();
129
130 // Use Kahan summation for the heck of it:
131 const double y = mag - residual;
132 const double t = result + y;
133 residual = (t - result) - y;
134 result = t;
135 }
136 return result;
137 }
138
139 template <typename T>
140 void
141 verifyProcrustes (const std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T>>& from,
142 const std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T>>& to)
143 {
144 const T eps = std::sqrt (std::numeric_limits<T>::epsilon());
145
146 const size_t n = from.size();
147
148 // Validate that passing in uniform weights gives the same answer as
149 // passing in no weights:
150 std::vector<T> weights (from.size());
151 for (size_t i = 0; i < weights.size(); ++i)
152 weights[i] = 1;
153 IMATH_INTERNAL_NAMESPACE::M44d m1 = procrustesRotationAndTranslation (&from[0], &to[0], n);
154 IMATH_INTERNAL_NAMESPACE::M44d m2 =
155 procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n);
156 for (int i = 0; i < 4; ++i)
157 for (int j = 0; j < 4; ++j)
158 assert (std::abs (m1[i][j] - m2[i][j]) < eps);
159
160 // Now try the weighted version:
161 for (size_t i = 0; i < weights.size (); ++i)
162 weights[i] = static_cast<T>(i + 1);
163
164 IMATH_INTERNAL_NAMESPACE::M44d m =
165 procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n);
166
167 // with scale:
168 IMATH_INTERNAL_NAMESPACE::M44d ms =
169 procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n, true);
170
171 // Verify that it's orthonormal w/ positive determinant.
172 const T det = static_cast<T>(m.determinant ());
173 assert (std::abs (det - T (1)) < eps);
174
175 // Verify orthonormal:
176 IMATH_INTERNAL_NAMESPACE::M33d upperLeft;
177 for (int i = 0; i < 3; ++i)
178 for (int j = 0; j < 3; ++j)
179 upperLeft[i][j] = m[i][j];
180 IMATH_INTERNAL_NAMESPACE::M33d product = upperLeft * upperLeft.transposed();
181 for (int i = 0; i < 3; ++i)
182 {
183 for (int j = 0; j < 3; ++j)
184 {
185 const double expected = (i == j ? 1.0 : 0.0);
186 assert (std::abs (product[i][j] - expected) < eps);
187 }
188 }
189
190 // Verify that nearby transforms are worse:
191 const size_t numTries = 10;
192 IMATH_INTERNAL_NAMESPACE::Rand48 rand (1056);
193 const double delta = 1e-3;
194 for (size_t i = 0; i < numTries; ++i)
195 {
196 // Construct an orthogonal rotation matrix using Euler angles:
197 IMATH_INTERNAL_NAMESPACE::Eulerd diffRot (delta * rand.nextf(),
198 delta * rand.nextf(),
199 delta * rand.nextf());
200
201 assert (procrustesError (&from[0], &to[0], &weights[0], n, m * diffRot.toMatrix44()) >
202 procrustesError (&from[0], &to[0], &weights[0], n, m));
203
204 // Try a small translation:
205 IMATH_INTERNAL_NAMESPACE::V3d diffTrans (delta * rand.nextf(),
206 delta * rand.nextf(),
207 delta * rand.nextf());
208 IMATH_INTERNAL_NAMESPACE::M44d translateMatrix;
209 translateMatrix.translate (diffTrans);
210 assert (procrustesError (&from[0], &to[0], &weights[0], n, m * translateMatrix) >
211 procrustesError (&from[0], &to[0], &weights[0], n, m));
212 }
213
214 // Try a small scale:
215 IMATH_INTERNAL_NAMESPACE::M44d newMat = ms;
216 const double scaleDiff = delta;
217 for (int i = 0; i < 3; ++i)
218 for (int j = 0; j < 3; ++j)
219 newMat[i][j] = ms[i][j] * (1.0 + scaleDiff);
220 assert (procrustesError (&from[0], &to[0], &weights[0], n, newMat) >
221 procrustesError (&from[0], &to[0], &weights[0], n, ms));
222
223 for (int i = 0; i < 3; ++i)
224 for (int j = 0; j < 3; ++j)
225 newMat[i][j] = ms[i][j] * (1.0 - scaleDiff);
226 assert (procrustesError (&from[0], &to[0], &weights[0], n, newMat) >
227 procrustesError (&from[0], &to[0], &weights[0], n, ms));
228
229 //
230 // Verify the magical property that makes shape springs work:
231 // when the displacements Q*A-B, times the weights,
232 // are applied as forces at B,
233 // there is zero net force and zero net torque.
234 //
235 {
236 IMATH_INTERNAL_NAMESPACE::V3d netForce (0);
237 IMATH_INTERNAL_NAMESPACE::V3d netTorque (0);
238 for (size_t iPoint = 0; iPoint < n; ++iPoint)
239 {
240 const IMATH_INTERNAL_NAMESPACE::V3d force =
241 weights[iPoint] * (from[iPoint] * m - to[iPoint]);
242 netForce += force;
243 netTorque += to[iPoint].cross (force);
244 }
245
246 assert (netForce.length2() < eps);
247 assert (netTorque.length2() < eps);
248 }
249 }
250
251 template <typename T>
252 void
253 testProcrustesWithMatrix (const IMATH_INTERNAL_NAMESPACE::M44d& m)
254 {
255 std::cout << "Testing Procrustes algorithm with arbitrary matrix: \n" << m;
256 std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T>> fromPoints;
257 std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T>> toPoints;
258
259 IMATH_INTERNAL_NAMESPACE::Rand48 random (1209);
260 std::cout << " numPoints: ";
261 for (size_t numPoints = 1; numPoints < 10; ++numPoints)
262 {
263 std::cout << numPoints << " " << std::flush;
264 fromPoints.clear();
265 toPoints.clear();
266 for (size_t i = 0; i < numPoints; ++i)
267 {
268 const IMATH_INTERNAL_NAMESPACE::V3d fromPt (random.nextf(),
269 random.nextf(),
270 random.nextf());
271 const IMATH_INTERNAL_NAMESPACE::V3d toPt = fromPt * m;
272 fromPoints.push_back (IMATH_INTERNAL_NAMESPACE::Vec3<T> (fromPt));
273 toPoints.push_back (IMATH_INTERNAL_NAMESPACE::Vec3<T> (toPt));
274 }
275 verifyProcrustes (fromPoints, toPoints);
276 }
277 std::cout << "OK\n";
278 }
279
280 template <typename T>
281 void
282 testProcrustesImp()
283 {
284 // Test the empty case:
285 IMATH_INTERNAL_NAMESPACE::M44d id =
286 procrustesRotationAndTranslation ((IMATH_INTERNAL_NAMESPACE::Vec3<T>*) 0,
287 (IMATH_INTERNAL_NAMESPACE::Vec3<T>*) 0,
288 (T*) 0,
289 0);
290 assert (id == IMATH_INTERNAL_NAMESPACE::M44d());
291
292 id = procrustesRotationAndTranslation ((IMATH_INTERNAL_NAMESPACE::Vec3<T>*) 0,
293 (IMATH_INTERNAL_NAMESPACE::Vec3<T>*) 0,
294 0);
295 assert (id == IMATH_INTERNAL_NAMESPACE::M44d());
296
297 // First we'll test with a bunch of known translation/rotation matrices
298 // to make sure we get back exactly the same points:
299 IMATH_INTERNAL_NAMESPACE::M44d m;
300 m.makeIdentity();
301 testTranslationRotationMatrix<T> (m);
302
303 m.translate (IMATH_INTERNAL_NAMESPACE::V3d (3.0, 5.0, -0.2));
304 testTranslationRotationMatrix<T> (m);
305
306 m.rotate (IMATH_INTERNAL_NAMESPACE::V3d (M_PI, 0, 0));
307 testTranslationRotationMatrix<T> (m);
308
309 m.rotate (IMATH_INTERNAL_NAMESPACE::V3d (0, M_PI / 4.0, 0));
310 testTranslationRotationMatrix<T> (m);
311
312 m.rotate (IMATH_INTERNAL_NAMESPACE::V3d (0, 0, -3.0 / 4.0 * M_PI));
313 testTranslationRotationMatrix<T> (m);
314
315 m.makeIdentity();
316 testWithTranslateRotateAndScale<T> (m);
317
318 m.translate (IMATH_INTERNAL_NAMESPACE::V3d (0.4, 6.0, 10.0));
319 testWithTranslateRotateAndScale<T> (m);
320
321 m.rotate (IMATH_INTERNAL_NAMESPACE::V3d (M_PI, 0, 0));
322 testWithTranslateRotateAndScale<T> (m);
323
324 m.rotate (IMATH_INTERNAL_NAMESPACE::V3d (0, M_PI / 4.0, 0));
325 testWithTranslateRotateAndScale<T> (m);
326
327 m.rotate (IMATH_INTERNAL_NAMESPACE::V3d (0, 0, -3.0 / 4.0 * M_PI));
328 testWithTranslateRotateAndScale<T> (m);
329
330 m.scale (IMATH_INTERNAL_NAMESPACE::V3d (2.0, 2.0, 2.0));
331 testWithTranslateRotateAndScale<T> (m);
332
333 m.scale (IMATH_INTERNAL_NAMESPACE::V3d (0.01, 0.01, 0.01));
334 testWithTranslateRotateAndScale<T> (m);
335
336 // Now we'll test with some random point sets and verify
337 // the various Procrustes properties:
338 std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T>> fromPoints;
339 std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T>> toPoints;
340 fromPoints.clear();
341 toPoints.clear();
342
343 for (size_t i = 0; i < 4; ++i)
344 {
345 const T theta = T (2 * i) / T (M_PI);
346 const T offset = T(M_PI / 3.0);
347 fromPoints.push_back (
348 IMATH_INTERNAL_NAMESPACE::Vec3<T> (cos (theta), sin (theta), 0));
349 toPoints.push_back (IMATH_INTERNAL_NAMESPACE::Vec3<T> (
350 cos (theta + offset), sin (theta + offset), 0));
351 }
352 verifyProcrustes (fromPoints, toPoints);
353
354 IMATH_INTERNAL_NAMESPACE::Rand48 random (1209);
355 for (size_t numPoints = 1; numPoints < 10; ++numPoints)
356 {
357 fromPoints.clear();
358 toPoints.clear();
359 for (size_t i = 0; i < numPoints; ++i)
360 {
361 fromPoints.push_back (IMATH_INTERNAL_NAMESPACE::Vec3<T> (
362 static_cast<T>(random.nextf ()),
363 static_cast<T>(random.nextf ()),
364 static_cast<T>(random.nextf ())));
365 toPoints.push_back (IMATH_INTERNAL_NAMESPACE::Vec3<T> (
366 static_cast<T>(random.nextf ()),
367 static_cast<T>(random.nextf ()),
368 static_cast<T>(random.nextf ())));
369 }
370 }
371 verifyProcrustes (fromPoints, toPoints);
372
373 // Test with some known matrices of varying degrees of quality:
374 testProcrustesWithMatrix<T> (m);
375
376 m.translate (IMATH_INTERNAL_NAMESPACE::Vec3<T> (3, 4, 1));
377 testProcrustesWithMatrix<T> (m);
378
379 m.translate (IMATH_INTERNAL_NAMESPACE::Vec3<T> (-10, 2, 1));
380 testProcrustesWithMatrix<T> (m);
381
382 IMATH_INTERNAL_NAMESPACE::Eulerd rot (M_PI / 3.0, 3.0 * M_PI / 4.0, 0);
383 m = m * rot.toMatrix44();
384 testProcrustesWithMatrix<T> (m);
385
386 m.scale (IMATH_INTERNAL_NAMESPACE::Vec3<T> (T(1.5), T(6.4), T(2.0)));
387 testProcrustesWithMatrix<T> (m);
388
389 IMATH_INTERNAL_NAMESPACE::Eulerd rot2 (1.0, M_PI, M_PI / 3.0);
390 m = m * rot.toMatrix44();
391
392 m.scale (IMATH_INTERNAL_NAMESPACE::Vec3<T> (-1, 1, 1));
393 testProcrustesWithMatrix<T> (m);
394
395 m.scale (IMATH_INTERNAL_NAMESPACE::Vec3<T> (1, T(0.001), 1));
396 testProcrustesWithMatrix<T> (m);
397
398 m.scale (IMATH_INTERNAL_NAMESPACE::Vec3<T> (1, 1, 0));
399 testProcrustesWithMatrix<T> (m);
400 }
401
402 void
403 testProcrustes()
404 {
405 std::cout << "Testing Procrustes algorithms in single precision..." << std::endl;
406 testProcrustesImp<float>();
407
408 std::cout << "Testing Procrustes algorithms in double precision..." << std::endl;
409 testProcrustesImp<double>();
410 }