Mercurial > hg > orthanc-stone

comparison Framework/Toolbox/LinearAlgebra.cpp @ 159:0a73d76333db wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
populating LinearAlgebra
author Sebastien Jodogne <s.jodogne@gmail.com>
date Wed, 14 Feb 2018 09:15:08 +0100
parents a053ca7fa5c6
children e9dae7e7bffc
comparison
equal deleted inserted replaced
158:a053ca7fa5c6 159:0a73d76333db
194 for (size_t i = 0; i < n; i++) 194 for (size_t i = 0; i < n; i++)
195 { 195 {
196 target(i, 0) = source[i]; 196 target(i, 0) = source[i];
197 } 197 }
198 } 198 }
199
200
201 double ComputeDeterminant(const Matrix& a)
202 {
203 if (a.size1() != a.size2())
204 {
205 LOG(ERROR) << "Determinant only exists for square matrices";
206 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
207 }
208
209 // https://en.wikipedia.org/wiki/Rule_of_Sarrus
210 if (a.size1() == 1)
211 {
212 return a(0,0);
213 }
214 else if (a.size1() == 2)
215 {
216 return a(0,0) * a(1,1) - a(0,1) * a(1,0);
217 }
218 else if (a.size1() == 3)
219 {
220 return (a(0,0) * a(1,1) * a(2,2) +
221 a(0,1) * a(1,2) * a(2,0) +
222 a(0,2) * a(1,0) * a(2,1) -
223 a(2,0) * a(1,1) * a(0,2) -
224 a(2,1) * a(1,2) * a(0,0) -
225 a(2,2) * a(1,0) * a(0,1));
226 }
227 else
228 {
229 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
230 }
231 }
232
233
234 bool IsOrthogonalMatrix(const Matrix& q,
235 double threshold)
236 {
237 // https://en.wikipedia.org/wiki/Orthogonal_matrix
238
239 using namespace boost::numeric::ublas;
240
241 const Matrix check = prod(trans(q), q) - identity_matrix<double>(3);
242
243 typename type_traits<double>::real_type norm = norm_inf(check);
244
245 return (norm <= threshold);
246 }
247
248
249 bool IsOrthogonalMatrix(const Matrix& q)
250 {
251 return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon());
252 }
253
254
255 bool IsRotationMatrix(const Matrix& r,
256 double threshold)
257 {
258 return (IsOrthogonalMatrix(r, threshold) &&
259 IsNear(ComputeDeterminant(r), 1.0, threshold));
260 }
261
262
263 bool IsRotationMatrix(const Matrix& r)
264 {
265 return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon());
266 }
267
268
269 void InvertUpperTriangularMatrix(Matrix& output,
270 const Matrix& k)
271 {
272 if (k.size1() != k.size2())
273 {
274 LOG(ERROR) << "Determinant only exists for square matrices";
275 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
276 }
277
278 output.resize(k.size1(), k.size2());
279
280 for (size_t i = 1; i < k.size1(); i++)
281 {
282 for (size_t j = 0; j < i; j++)
283 {
284 if (!IsCloseToZero(k(i, j)))
285 {
286 LOG(ERROR) << "Not an upper triangular matrix";
287 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
288 }
289
290 output(i, j) = 0; // The output is also upper triangular
291 }
292 }
293
294 if (k.size1() == 3)
295 {
296 // https://math.stackexchange.com/a/1004181
297 double a = k(0, 0);
298 double b = k(0, 1);
299 double c = k(0, 2);
300 double d = k(1, 1);
301 double e = k(1, 2);
302 double f = k(2, 2);
303
304 if (IsCloseToZero(a) ||
305 IsCloseToZero(d) ||
306 IsCloseToZero(f))
307 {
308 LOG(ERROR) << "Singular upper triangular matrix";
309 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
310 }
311 else
312 {
313 output(0, 0) = 1.0 / a;
314 output(0, 1) = -b / (a * d);
315 output(0, 2) = (b * e - c * d) / (a * f * d);
316 output(1, 1) = 1.0 / d;
317 output(1, 2) = -e / (f * d);
318 output(2, 2) = 1.0 / f;
319 }
320 }
321 else
322 {
323 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
324 }
325 }
326
327
328 static void GetGivensComponent(double& c,
329 double& s,
330 const Matrix& a,
331 size_t i,
332 size_t j)
333 {
334 assert(i < 3 && j < 3);
335
336 double x = a(i, i);
337 double y = a(i, j);
338 double n = sqrt(x * x + y * y);
339
340 if (IsCloseToZero(n))
341 {
342 c = 1;
343 s = 0;
344 }
345 else
346 {
347 c = x / n;
348 s = -y / n;
349 }
350 }
351
352
353 /**
354 * This function computes the RQ decomposition of a 3x3 matrix,
355 * using Givens rotations. Reference: Algorithm A4.1 (page 579) of
356 * "Multiple View Geometry in Computer Vision" (2nd edition). The
357 * output matrix "Q" is a rotation matrix, and "R" is upper
358 * triangular.
359 **/
360 void RQDecomposition3x3(Matrix& r,
361 Matrix& q,
362 const Matrix& a)
363 {
364 using namespace boost::numeric::ublas;
365
366 if (a.size1() != 3 ||
367 a.size2() != 3)
368 {
369 LOG(ERROR) << "Only applicable to a 3x3 matrix";
370 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
371 }
372
373 r.resize(3, 3);
374 q.resize(3, 3);
375
376 r = a;
377 q = identity_matrix<double>(3);
378
379 {
380 // Set A(2,1) to zero
381 double c, s;
382 GetGivensComponent(c, s, r, 2, 1);
383
384 double v[9] = { 1, 0, 0,
385 0, c, -s,
386 0, s, c };
387
388 Matrix g;
389 FillMatrix(g, 3, 3, v);
390
391 r = prod(r, g);
392 q = prod(trans(g), q);
393 }
394
395
396 {
397 // Set A(2,0) to zero
398 double c, s;
399 GetGivensComponent(c, s, r, 2, 0);
400
401 double v[9] = { c, 0, -s,
402 0, 1, 0,
403 s, 0, c };
404
405 Matrix g;
406 FillMatrix(g, 3, 3, v);
407
408 r = prod(r, g);
409 q = prod(trans(g), q);
410 }
411
412
413 {
414 // Set A(1,0) to zero
415 double c, s;
416 GetGivensComponent(c, s, r, 1, 0);
417
418 double v[9] = { c, -s, 0,
419 s, c, 0,
420 0, 0, 1 };
421
422 Matrix g;
423 FillMatrix(g, 3, 3, v);
424
425 r = prod(r, g);
426 q = prod(trans(g), q);
427 }
428
429 if (!IsCloseToZero(norm_inf(prod(r, q) - a)) ||
430 !IsRotationMatrix(q) ||
431 !IsCloseToZero(r(1, 0)) ||
432 !IsCloseToZero(r(2, 0)) ||
433 !IsCloseToZero(r(2, 1)))
434 {
435 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
436 }
437 }
199 } 438 }
200 } 439 }