Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/LinearAlgebra.cpp @ 165:8d50e6be565d wasm
LinearAlgebra toolbox
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Thu, 15 Feb 2018 13:51:29 +0100 |
parents | 432b1f812d14 |
children | 4f661e2f7b6c |
comparison
equal
deleted
inserted
replaced
164:432b1f812d14 | 165:8d50e6be565d |
---|---|
103 v[1] = v2; | 103 v[1] = v2; |
104 } | 104 } |
105 | 105 |
106 | 106 |
107 void AssignVector(Vector& v, | 107 void AssignVector(Vector& v, |
108 double v1) | |
109 { | |
110 v.resize(1); | |
111 v[0] = v1; | |
112 } | |
113 | |
114 | |
115 void AssignVector(Vector& v, | |
108 double v1, | 116 double v1, |
109 double v2, | 117 double v2, |
110 double v3) | 118 double v3) |
111 { | 119 { |
112 v.resize(3); | 120 v.resize(3); |
125 v.resize(4); | 133 v.resize(4); |
126 v[0] = v1; | 134 v[0] = v1; |
127 v[1] = v2; | 135 v[1] = v2; |
128 v[2] = v3; | 136 v[2] = v3; |
129 v[3] = v4; | 137 v[3] = v4; |
138 } | |
139 | |
140 | |
141 Vector CreateVector(double v1) | |
142 { | |
143 Vector v; | |
144 AssignVector(v, v1); | |
145 return v; | |
146 } | |
147 | |
148 | |
149 Vector CreateVector(double v1, | |
150 double v2) | |
151 { | |
152 Vector v; | |
153 AssignVector(v, v1, v2); | |
154 return v; | |
155 } | |
156 | |
157 | |
158 Vector CreateVector(double v1, | |
159 double v2, | |
160 double v3) | |
161 { | |
162 Vector v; | |
163 AssignVector(v, v1, v2, v3); | |
164 return v; | |
165 } | |
166 | |
167 | |
168 Vector CreateVector(double v1, | |
169 double v2, | |
170 double v3, | |
171 double v4) | |
172 { | |
173 Vector v; | |
174 AssignVector(v, v1, v2, v3, v4); | |
175 return v; | |
130 } | 176 } |
131 | 177 |
132 | 178 |
133 bool IsNear(double x, | 179 bool IsNear(double x, |
134 double y) | 180 double y) |
541 | 587 |
542 target = boost::numeric::ublas::identity_matrix<double>(source.size1()); | 588 target = boost::numeric::ublas::identity_matrix<double>(source.size1()); |
543 lu_substitute(a, permutation, target); | 589 lu_substitute(a, permutation, target); |
544 } | 590 } |
545 } | 591 } |
592 | |
593 | |
594 void CreateSkewSymmetric(Matrix& s, | |
595 const Vector& v) | |
596 { | |
597 assert(v.size() == 3); | |
598 | |
599 s.resize(3, 3); | |
600 s(0,0) = 0; | |
601 s(0,1) = -v[2]; | |
602 s(0,2) = v[1]; | |
603 s(1,0) = v[2]; | |
604 s(1,1) = 0; | |
605 s(1,2) = -v[0]; | |
606 s(2,0) = -v[1]; | |
607 s(2,1) = v[0]; | |
608 s(2,2) = 0; | |
609 } | |
610 | |
611 | |
612 void AlignVectorsWithRotation(Matrix& r, | |
613 const Vector& a, | |
614 const Vector& b) | |
615 { | |
616 // This is Rodrigues' rotation formula: | |
617 // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation | |
618 | |
619 // Check also result A4.6 from "Multiple View Geometry in Computer | |
620 // Vision - 2nd edition" (p. 584) | |
621 | |
622 if (a.size() != 3 || | |
623 b.size() != 3) | |
624 { | |
625 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
626 } | |
627 | |
628 double aNorm = boost::numeric::ublas::norm_2(a); | |
629 double bNorm = boost::numeric::ublas::norm_2(b); | |
630 | |
631 if (LinearAlgebra::IsCloseToZero(aNorm) || | |
632 LinearAlgebra::IsCloseToZero(bNorm)) | |
633 { | |
634 LOG(ERROR) << "Vector with zero norm"; | |
635 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
636 } | |
637 | |
638 Vector aUnit, bUnit; | |
639 aUnit = a / aNorm; | |
640 bUnit = b / bNorm; | |
641 | |
642 Vector v; | |
643 LinearAlgebra::CrossProduct(v, aUnit, bUnit); | |
644 | |
645 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit); | |
646 | |
647 if (LinearAlgebra::IsCloseToZero(1 + cosine)) | |
648 { | |
649 // "a == -b": TODO | |
650 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
651 } | |
652 | |
653 Matrix k; | |
654 CreateSkewSymmetric(k, v); | |
655 | |
656 #if 0 | |
657 double sine = boost::numeric::ublas::norm_2(v); | |
658 | |
659 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
660 sine * k + | |
661 (1 - cosine) * boost::numeric::ublas::prod(k, k)); | |
662 #else | |
663 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
664 k + | |
665 boost::numeric::ublas::prod(k, k) / (1 + cosine)); | |
666 #endif | |
667 } | |
668 | |
669 | |
670 Matrix InvertScaleTranslationMatrix(const Matrix& t) | |
671 { | |
672 if (t.size1() != 4 || | |
673 t.size2() != 4 || | |
674 !LinearAlgebra::IsCloseToZero(t(0,1)) || | |
675 !LinearAlgebra::IsCloseToZero(t(0,2)) || | |
676 !LinearAlgebra::IsCloseToZero(t(1,0)) || | |
677 !LinearAlgebra::IsCloseToZero(t(1,2)) || | |
678 !LinearAlgebra::IsCloseToZero(t(2,0)) || | |
679 !LinearAlgebra::IsCloseToZero(t(2,1)) || | |
680 !LinearAlgebra::IsCloseToZero(t(3,0)) || | |
681 !LinearAlgebra::IsCloseToZero(t(3,1)) || | |
682 !LinearAlgebra::IsCloseToZero(t(3,2))) | |
683 { | |
684 LOG(ERROR) << "This matrix is more than a zoom/translate transform"; | |
685 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
686 } | |
687 | |
688 const double sx = t(0,0); | |
689 const double sy = t(1,1); | |
690 const double sz = t(2,2); | |
691 const double w = t(3,3); | |
692 | |
693 if (LinearAlgebra::IsCloseToZero(sx) || | |
694 LinearAlgebra::IsCloseToZero(sy) || | |
695 LinearAlgebra::IsCloseToZero(sz) || | |
696 LinearAlgebra::IsCloseToZero(w)) | |
697 { | |
698 LOG(ERROR) << "Singular transform"; | |
699 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
700 } | |
701 | |
702 const double tx = t(0,3); | |
703 const double ty = t(1,3); | |
704 const double tz = t(2,3); | |
705 | |
706 Matrix m = IdentityMatrix(4); | |
707 | |
708 m(0,0) = 1.0 / sx; | |
709 m(1,1) = 1.0 / sy; | |
710 m(2,2) = 1.0 / sz; | |
711 m(3,3) = 1.0 / w; | |
712 | |
713 m(0,3) = -tx / (sx * w); | |
714 m(1,3) = -ty / (sy * w); | |
715 m(2,3) = -tz / (sz * w); | |
716 | |
717 return m; | |
718 } | |
719 | |
720 | |
721 bool IsShearMatrix(const Matrix& shear) | |
722 { | |
723 return (shear.size1() == 4 && | |
724 shear.size2() == 4 && | |
725 LinearAlgebra::IsNear(1.0, shear(0,0)) && | |
726 LinearAlgebra::IsNear(0.0, shear(0,1)) && | |
727 LinearAlgebra::IsNear(0.0, shear(0,3)) && | |
728 LinearAlgebra::IsNear(0.0, shear(1,0)) && | |
729 LinearAlgebra::IsNear(1.0, shear(1,1)) && | |
730 LinearAlgebra::IsNear(0.0, shear(1,3)) && | |
731 LinearAlgebra::IsNear(0.0, shear(2,0)) && | |
732 LinearAlgebra::IsNear(0.0, shear(2,1)) && | |
733 LinearAlgebra::IsNear(1.0, shear(2,2)) && | |
734 LinearAlgebra::IsNear(0.0, shear(2,3)) && | |
735 LinearAlgebra::IsNear(0.0, shear(3,0)) && | |
736 LinearAlgebra::IsNear(0.0, shear(3,1)) && | |
737 LinearAlgebra::IsNear(1.0, shear(3,3))); | |
738 } | |
739 | |
740 | |
741 Matrix InvertShearMatrix(const Matrix& shear) | |
742 { | |
743 if (!IsShearMatrix(shear)) | |
744 { | |
745 LOG(ERROR) << "Not a valid shear matrix"; | |
746 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
747 } | |
748 | |
749 Matrix m = IdentityMatrix(4); | |
750 m(0,2) = -shear(0,2); | |
751 m(1,2) = -shear(1,2); | |
752 m(3,2) = -shear(3,2); | |
753 | |
754 return m; | |
755 } | |
546 } | 756 } |
547 } | 757 } |