Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/LinearAlgebra.cpp @ 189:964118e7e6de wasm
unit test for AlignVectorsWithRotation
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Fri, 16 Mar 2018 13:19:23 +0100 |
parents | ff8556874557 |
children | e9c7a78a3e77 |
comparison
equal
deleted
inserted
replaced
188:45b03b04a777 | 189:964118e7e6de |
---|---|
535 s(2,1) = v[0]; | 535 s(2,1) = v[0]; |
536 s(2,2) = 0; | 536 s(2,2) = 0; |
537 } | 537 } |
538 | 538 |
539 | 539 |
540 void AlignVectorsWithRotation(Matrix& r, | |
541 const Vector& a, | |
542 const Vector& b) | |
543 { | |
544 // This is Rodrigues' rotation formula: | |
545 // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation | |
546 | |
547 // Check also result A4.6 from "Multiple View Geometry in Computer | |
548 // Vision - 2nd edition" (p. 584) | |
549 | |
550 if (a.size() != 3 || | |
551 b.size() != 3) | |
552 { | |
553 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
554 } | |
555 | |
556 double aNorm = boost::numeric::ublas::norm_2(a); | |
557 double bNorm = boost::numeric::ublas::norm_2(b); | |
558 | |
559 if (LinearAlgebra::IsCloseToZero(aNorm) || | |
560 LinearAlgebra::IsCloseToZero(bNorm)) | |
561 { | |
562 LOG(ERROR) << "Vector with zero norm"; | |
563 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
564 } | |
565 | |
566 Vector aUnit, bUnit; | |
567 aUnit = a / aNorm; | |
568 bUnit = b / bNorm; | |
569 | |
570 Vector v; | |
571 LinearAlgebra::CrossProduct(v, aUnit, bUnit); | |
572 | |
573 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit); | |
574 | |
575 if (LinearAlgebra::IsCloseToZero(1 + cosine)) | |
576 { | |
577 // "a == -b": TODO | |
578 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
579 } | |
580 | |
581 Matrix k; | |
582 CreateSkewSymmetric(k, v); | |
583 | |
584 #if 0 | |
585 double sine = boost::numeric::ublas::norm_2(v); | |
586 | |
587 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
588 sine * k + | |
589 (1 - cosine) * boost::numeric::ublas::prod(k, k)); | |
590 #else | |
591 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
592 k + | |
593 boost::numeric::ublas::prod(k, k) / (1 + cosine)); | |
594 #endif | |
595 } | |
596 | |
597 | |
598 Matrix InvertScalingTranslationMatrix(const Matrix& t) | 540 Matrix InvertScalingTranslationMatrix(const Matrix& t) |
599 { | 541 { |
600 if (t.size1() != 4 || | 542 if (t.size1() != 4 || |
601 t.size2() != 4 || | 543 t.size2() != 4 || |
602 !LinearAlgebra::IsCloseToZero(t(0,1)) || | 544 !LinearAlgebra::IsCloseToZero(t(0,1)) || |