Mercurial > hg > orthanc-stone

comparison Framework/Toolbox/GeometryToolbox.cpp @ 189:964118e7e6de wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
unit test for AlignVectorsWithRotation
author Sebastien Jodogne <s.jodogne@gmail.com>
date Fri, 16 Mar 2018 13:19:23 +0100
parents 8d50e6be565d
children fccffbf99ba1
comparison
equal deleted inserted replaced
188:45b03b04a777 189:964118e7e6de
413 413
414 p = origin + t * direction; 414 p = origin + t * direction;
415 return true; 415 return true;
416 } 416 }
417 } 417 }
418
419
420 void AlignVectorsWithRotation(Matrix& r,
421 const Vector& a,
422 const Vector& b)
423 {
424 // This is Rodrigues' rotation formula:
425 // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
426
427 // Check also result A4.6 from "Multiple View Geometry in Computer
428 // Vision - 2nd edition" (p. 584)
429
430 if (a.size() != 3 ||
431 b.size() != 3)
432 {
433 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
434 }
435
436 double aNorm = boost::numeric::ublas::norm_2(a);
437 double bNorm = boost::numeric::ublas::norm_2(b);
438
439 if (LinearAlgebra::IsCloseToZero(aNorm) ||
440 LinearAlgebra::IsCloseToZero(bNorm))
441 {
442 LOG(ERROR) << "Vector with zero norm";
443 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
444 }
445
446 Vector aUnit, bUnit;
447 aUnit = a / aNorm;
448 bUnit = b / bNorm;
449
450 Vector v;
451 LinearAlgebra::CrossProduct(v, aUnit, bUnit);
452
453 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
454
455 if (LinearAlgebra::IsCloseToZero(1 + cosine))
456 {
457 // "a == -b": TODO
458 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
459 }
460
461 Matrix k;
462 LinearAlgebra::CreateSkewSymmetric(k, v);
463
464 #if 0
465 double sine = boost::numeric::ublas::norm_2(v);
466
467 r = (boost::numeric::ublas::identity_matrix<double>(3) +
468 sine * k +
469 (1 - cosine) * boost::numeric::ublas::prod(k, k));
470 #else
471 r = (boost::numeric::ublas::identity_matrix<double>(3) +
472 k +
473 boost::numeric::ublas::prod(k, k) / (1 + cosine));
474 #endif
475 }
418 } 476 }
419 } 477 }