Mercurial > hg > orthanc-stone

comparison Framework/Toolbox/LinearAlgebra.cpp @ 165:8d50e6be565d wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 }