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)) || |