Mercurial > hg > orthanc-stone
annotate Framework/Toolbox/LinearAlgebra.cpp @ 495:6405435480ae bgo-commands-codegen
Fixed template to add dump capabilities + started work on an integrated TS/WASM test
author | bgo-osimis |
---|---|
date | Sat, 23 Feb 2019 14:14:32 +0100 |
parents | b70e9be013e4 |
children | 5aa728500586 |
rev | line source |
---|---|
158 | 1 /** |
2 * Stone of Orthanc | |
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics | |
4 * Department, University Hospital of Liege, Belgium | |
439 | 5 * Copyright (C) 2017-2019 Osimis S.A., Belgium |
158 | 6 * |
7 * This program is free software: you can redistribute it and/or | |
8 * modify it under the terms of the GNU Affero General Public License | |
9 * as published by the Free Software Foundation, either version 3 of | |
10 * the License, or (at your option) any later version. | |
11 * | |
12 * This program is distributed in the hope that it will be useful, but | |
13 * WITHOUT ANY WARRANTY; without even the implied warranty of | |
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
15 * Affero General Public License for more details. | |
16 * | |
17 * You should have received a copy of the GNU Affero General Public License | |
18 * along with this program. If not, see <http://www.gnu.org/licenses/>. | |
19 **/ | |
20 | |
21 | |
22 #include "LinearAlgebra.h" | |
23 | |
212
5412adf19980
resort to OrthancFramework
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
201
diff
changeset
|
24 #include <Core/Logging.h> |
5412adf19980
resort to OrthancFramework
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
201
diff
changeset
|
25 #include <Core/OrthancException.h> |
5412adf19980
resort to OrthancFramework
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
201
diff
changeset
|
26 #include <Core/Toolbox.h> |
158 | 27 |
28 #include <stdio.h> | |
29 #include <boost/lexical_cast.hpp> | |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
30 #include <boost/numeric/ublas/lu.hpp> |
158 | 31 |
32 namespace OrthancStone | |
33 { | |
34 namespace LinearAlgebra | |
35 { | |
36 void Print(const Vector& v) | |
37 { | |
38 for (size_t i = 0; i < v.size(); i++) | |
39 { | |
40 printf("%g\n", v[i]); | |
41 //printf("%8.2f\n", v[i]); | |
42 } | |
43 printf("\n"); | |
44 } | |
45 | |
46 | |
47 void Print(const Matrix& m) | |
48 { | |
49 for (size_t i = 0; i < m.size1(); i++) | |
50 { | |
51 for (size_t j = 0; j < m.size2(); j++) | |
52 { | |
53 printf("%g ", m(i,j)); | |
54 //printf("%8.2f ", m(i,j)); | |
55 } | |
56 printf("\n"); | |
57 } | |
58 printf("\n"); | |
59 } | |
60 | |
61 | |
62 bool ParseVector(Vector& target, | |
63 const std::string& value) | |
64 { | |
65 std::vector<std::string> items; | |
66 Orthanc::Toolbox::TokenizeString(items, value, '\\'); | |
67 | |
68 target.resize(items.size()); | |
69 | |
70 for (size_t i = 0; i < items.size(); i++) | |
71 { | |
72 try | |
73 { | |
74 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i])); | |
75 } | |
76 catch (boost::bad_lexical_cast&) | |
77 { | |
78 target.clear(); | |
79 return false; | |
80 } | |
81 } | |
82 | |
83 return true; | |
84 } | |
85 | |
86 | |
87 bool ParseVector(Vector& target, | |
88 const Orthanc::DicomMap& dataset, | |
89 const Orthanc::DicomTag& tag) | |
90 { | |
91 std::string value; | |
92 return (dataset.CopyToString(value, tag, false) && | |
93 ParseVector(target, value)); | |
94 } | |
95 | |
96 | |
97 void NormalizeVector(Vector& u) | |
98 { | |
99 double norm = boost::numeric::ublas::norm_2(u); | |
100 if (!IsCloseToZero(norm)) | |
101 { | |
102 u = u / norm; | |
103 } | |
104 } | |
105 | |
106 | |
107 void CrossProduct(Vector& result, | |
108 const Vector& u, | |
109 const Vector& v) | |
110 { | |
111 if (u.size() != 3 || | |
112 v.size() != 3) | |
113 { | |
114 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
115 } | |
116 | |
117 result.resize(3); | |
118 | |
119 result[0] = u[1] * v[2] - u[2] * v[1]; | |
120 result[1] = u[2] * v[0] - u[0] * v[2]; | |
121 result[2] = u[0] * v[1] - u[1] * v[0]; | |
122 } | |
123 | |
124 | |
125 void FillMatrix(Matrix& target, | |
126 size_t rows, | |
127 size_t columns, | |
128 const double values[]) | |
129 { | |
130 target.resize(rows, columns); | |
131 | |
132 size_t index = 0; | |
133 | |
134 for (size_t y = 0; y < rows; y++) | |
135 { | |
136 for (size_t x = 0; x < columns; x++, index++) | |
137 { | |
138 target(y, x) = values[index]; | |
139 } | |
140 } | |
141 } | |
142 | |
143 | |
144 void FillVector(Vector& target, | |
145 size_t size, | |
146 const double values[]) | |
147 { | |
148 target.resize(size); | |
149 | |
150 for (size_t i = 0; i < size; i++) | |
151 { | |
152 target[i] = values[i]; | |
153 } | |
154 } | |
155 | |
156 | |
157 void Convert(Matrix& target, | |
158 const Vector& source) | |
159 { | |
160 const size_t n = source.size(); | |
161 | |
162 target.resize(n, 1); | |
163 | |
164 for (size_t i = 0; i < n; i++) | |
165 { | |
166 target(i, 0) = source[i]; | |
167 } | |
168 } | |
159
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
169 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
170 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
171 double ComputeDeterminant(const Matrix& a) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
172 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
173 if (a.size1() != a.size2()) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
174 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
175 LOG(ERROR) << "Determinant only exists for square matrices"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
176 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
177 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
178 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
179 // https://en.wikipedia.org/wiki/Rule_of_Sarrus |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
180 if (a.size1() == 1) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
181 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
182 return a(0,0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
183 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
184 else if (a.size1() == 2) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
185 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
186 return a(0,0) * a(1,1) - a(0,1) * a(1,0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
187 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
188 else if (a.size1() == 3) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
189 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
190 return (a(0,0) * a(1,1) * a(2,2) + |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
191 a(0,1) * a(1,2) * a(2,0) + |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
192 a(0,2) * a(1,0) * a(2,1) - |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
193 a(2,0) * a(1,1) * a(0,2) - |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
194 a(2,1) * a(1,2) * a(0,0) - |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
195 a(2,2) * a(1,0) * a(0,1)); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
196 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
197 else |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
198 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
199 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
200 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
201 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
202 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
203 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
204 bool IsOrthogonalMatrix(const Matrix& q, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
205 double threshold) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
206 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
207 // https://en.wikipedia.org/wiki/Orthogonal_matrix |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
208 |
392 | 209 if (q.size1() != q.size2()) |
210 { | |
211 LOG(ERROR) << "An orthogonal matrix must be squared"; | |
212 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
213 } | |
214 | |
159
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
215 using namespace boost::numeric::ublas; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
216 |
392 | 217 const Matrix check = prod(trans(q), q) - identity_matrix<double>(q.size1()); |
159
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
218 |
160 | 219 type_traits<double>::real_type norm = norm_inf(check); |
159
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
220 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
221 return (norm <= threshold); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
222 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
223 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
224 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
225 bool IsOrthogonalMatrix(const Matrix& q) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
226 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
227 return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon()); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
228 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
229 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
230 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
231 bool IsRotationMatrix(const Matrix& r, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
232 double threshold) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
233 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
234 return (IsOrthogonalMatrix(r, threshold) && |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
235 IsNear(ComputeDeterminant(r), 1.0, threshold)); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
236 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
237 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
238 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
239 bool IsRotationMatrix(const Matrix& r) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
240 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
241 return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon()); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
242 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
243 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
244 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
245 void InvertUpperTriangularMatrix(Matrix& output, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
246 const Matrix& k) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
247 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
248 if (k.size1() != k.size2()) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
249 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
250 LOG(ERROR) << "Determinant only exists for square matrices"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
251 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
252 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
253 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
254 output.resize(k.size1(), k.size2()); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
255 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
256 for (size_t i = 1; i < k.size1(); i++) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
257 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
258 for (size_t j = 0; j < i; j++) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
259 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
260 if (!IsCloseToZero(k(i, j))) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
261 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
262 LOG(ERROR) << "Not an upper triangular matrix"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
263 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
264 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
265 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
266 output(i, j) = 0; // The output is also upper triangular |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
267 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
268 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
269 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
270 if (k.size1() == 3) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
271 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
272 // https://math.stackexchange.com/a/1004181 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
273 double a = k(0, 0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
274 double b = k(0, 1); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
275 double c = k(0, 2); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
276 double d = k(1, 1); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
277 double e = k(1, 2); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
278 double f = k(2, 2); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
279 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
280 if (IsCloseToZero(a) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
281 IsCloseToZero(d) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
282 IsCloseToZero(f)) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
283 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
284 LOG(ERROR) << "Singular upper triangular matrix"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
285 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
286 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
287 else |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
288 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
289 output(0, 0) = 1.0 / a; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
290 output(0, 1) = -b / (a * d); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
291 output(0, 2) = (b * e - c * d) / (a * f * d); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
292 output(1, 1) = 1.0 / d; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
293 output(1, 2) = -e / (f * d); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
294 output(2, 2) = 1.0 / f; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
295 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
296 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
297 else |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
298 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
299 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
300 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
301 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
302 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
303 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
304 static void GetGivensComponent(double& c, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
305 double& s, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
306 const Matrix& a, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
307 size_t i, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
308 size_t j) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
309 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
310 assert(i < 3 && j < 3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
311 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
312 double x = a(i, i); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
313 double y = a(i, j); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
314 double n = sqrt(x * x + y * y); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
315 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
316 if (IsCloseToZero(n)) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
317 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
318 c = 1; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
319 s = 0; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
320 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
321 else |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
322 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
323 c = x / n; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
324 s = -y / n; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
325 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
326 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
327 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
328 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
329 /** |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
330 * This function computes the RQ decomposition of a 3x3 matrix, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
331 * using Givens rotations. Reference: Algorithm A4.1 (page 579) of |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
332 * "Multiple View Geometry in Computer Vision" (2nd edition). The |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
333 * output matrix "Q" is a rotation matrix, and "R" is upper |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
334 * triangular. |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
335 **/ |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
336 void RQDecomposition3x3(Matrix& r, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
337 Matrix& q, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
338 const Matrix& a) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
339 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
340 using namespace boost::numeric::ublas; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
341 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
342 if (a.size1() != 3 || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
343 a.size2() != 3) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
344 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
345 LOG(ERROR) << "Only applicable to a 3x3 matrix"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
346 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
347 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
348 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
349 r.resize(3, 3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
350 q.resize(3, 3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
351 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
352 r = a; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
353 q = identity_matrix<double>(3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
354 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
355 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
356 // Set A(2,1) to zero |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
357 double c, s; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
358 GetGivensComponent(c, s, r, 2, 1); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
359 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
360 double v[9] = { 1, 0, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
361 0, c, -s, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
362 0, s, c }; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
363 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
364 Matrix g; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
365 FillMatrix(g, 3, 3, v); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
366 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
367 r = prod(r, g); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
368 q = prod(trans(g), q); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
369 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
370 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
371 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
372 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
373 // Set A(2,0) to zero |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
374 double c, s; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
375 GetGivensComponent(c, s, r, 2, 0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
376 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
377 double v[9] = { c, 0, -s, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
378 0, 1, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
379 s, 0, c }; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
380 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
381 Matrix g; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
382 FillMatrix(g, 3, 3, v); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
383 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
384 r = prod(r, g); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
385 q = prod(trans(g), q); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
386 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
387 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
388 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
389 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
390 // Set A(1,0) to zero |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
391 double c, s; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
392 GetGivensComponent(c, s, r, 1, 0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
393 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
394 double v[9] = { c, -s, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
395 s, c, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
396 0, 0, 1 }; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
397 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
398 Matrix g; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
399 FillMatrix(g, 3, 3, v); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
400 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
401 r = prod(r, g); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
402 q = prod(trans(g), q); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
403 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
404 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
405 if (!IsCloseToZero(norm_inf(prod(r, q) - a)) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
406 !IsRotationMatrix(q) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
407 !IsCloseToZero(r(1, 0)) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
408 !IsCloseToZero(r(2, 0)) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
409 !IsCloseToZero(r(2, 1))) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
410 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
411 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
412 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
413 } |
163
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
414 |
169 | 415 |
416 bool InvertMatrixUnsafe(Matrix& target, | |
417 const Matrix& source) | |
163
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
418 { |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
419 if (source.size1() != source.size2()) |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
420 { |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
421 LOG(ERROR) << "Inverse only exists for square matrices"; |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
422 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
423 } |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
424 |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
425 if (source.size1() < 4) |
163
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
426 { |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
427 // For matrices with size below 4, use direct computations |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
428 // instead of LU decomposition |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
429 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
430 if (source.size1() == 0) |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
431 { |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
432 // By convention, the inverse of the empty matrix, is itself the empty matrix |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
433 target.resize(0, 0); |
169 | 434 return true; |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
435 } |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
436 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
437 double determinant = ComputeDeterminant(source); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
438 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
439 if (IsCloseToZero(determinant)) |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
440 { |
169 | 441 return false; |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
442 } |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
443 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
444 double denominator = 1.0 / determinant; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
445 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
446 target.resize(source.size1(), source.size2()); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
447 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
448 if (source.size1() == 1) |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
449 { |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
450 target(0, 0) = denominator; |
169 | 451 |
452 return true; | |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
453 } |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
454 else if (source.size1() == 2) |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
455 { |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
456 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
457 target(0, 0) = source(1, 1) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
458 target(0, 1) = -source(0, 1) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
459 target(1, 0) = -source(1, 0) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
460 target(1, 1) = source(0, 0) * denominator; |
169 | 461 |
462 return true; | |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
463 } |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
464 else if (source.size1() == 3) |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
465 { |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
466 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
467 const double a = source(0, 0); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
468 const double b = source(0, 1); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
469 const double c = source(0, 2); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
470 const double d = source(1, 0); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
471 const double e = source(1, 1); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
472 const double f = source(1, 2); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
473 const double g = source(2, 0); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
474 const double h = source(2, 1); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
475 const double i = source(2, 2); |
163
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
476 |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
477 target(0, 0) = (e * i - f * h) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
478 target(0, 1) = -(b * i - c * h) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
479 target(0, 2) = (b * f - c * e) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
480 target(1, 0) = -(d * i - f * g) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
481 target(1, 1) = (a * i - c * g) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
482 target(1, 2) = -(a * f - c * d) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
483 target(2, 0) = (d * h - e * g) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
484 target(2, 1) = -(a * h - b * g) * denominator; |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
485 target(2, 2) = (a * e - b * d) * denominator; |
169 | 486 |
487 return true; | |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
488 } |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
489 else |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
490 { |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
491 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
492 } |
163
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
493 } |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
494 else |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
495 { |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
496 // General case, using LU decomposition |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
497 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
498 Matrix a = source; // Copy the source matrix, as "lu_factorize()" modifies it |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
499 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
500 boost::numeric::ublas::permutation_matrix<size_t> permutation(source.size1()); |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
501 |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
502 if (boost::numeric::ublas::lu_factorize(a, permutation) != 0) |
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
503 { |
169 | 504 return false; |
505 } | |
506 else | |
507 { | |
508 target = boost::numeric::ublas::identity_matrix<double>(source.size1()); | |
509 lu_substitute(a, permutation, target); | |
510 return true; | |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
511 } |
169 | 512 } |
513 } | |
514 | |
164
432b1f812d14
inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
163
diff
changeset
|
515 |
169 | 516 |
517 void InvertMatrix(Matrix& target, | |
518 const Matrix& source) | |
519 { | |
520 if (!InvertMatrixUnsafe(target, source)) | |
521 { | |
522 LOG(ERROR) << "Cannot invert singular matrix"; | |
523 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
163
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
524 } |
8c5b24892ed2
LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
525 } |
165
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
526 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
527 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
528 void CreateSkewSymmetric(Matrix& s, |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
529 const Vector& v) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
530 { |
392 | 531 if (v.size() != 3) |
532 { | |
533 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
534 } | |
165
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
535 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
536 s.resize(3, 3); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
537 s(0,0) = 0; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
538 s(0,1) = -v[2]; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
539 s(0,2) = v[1]; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
540 s(1,0) = v[2]; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
541 s(1,1) = 0; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
542 s(1,2) = -v[0]; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
543 s(2,0) = -v[1]; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
544 s(2,1) = v[0]; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
545 s(2,2) = 0; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
546 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
547 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
548 |
166 | 549 Matrix InvertScalingTranslationMatrix(const Matrix& t) |
165
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
550 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
551 if (t.size1() != 4 || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
552 t.size2() != 4 || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
553 !LinearAlgebra::IsCloseToZero(t(0,1)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
554 !LinearAlgebra::IsCloseToZero(t(0,2)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
555 !LinearAlgebra::IsCloseToZero(t(1,0)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
556 !LinearAlgebra::IsCloseToZero(t(1,2)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
557 !LinearAlgebra::IsCloseToZero(t(2,0)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
558 !LinearAlgebra::IsCloseToZero(t(2,1)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
559 !LinearAlgebra::IsCloseToZero(t(3,0)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
560 !LinearAlgebra::IsCloseToZero(t(3,1)) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
561 !LinearAlgebra::IsCloseToZero(t(3,2))) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
562 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
563 LOG(ERROR) << "This matrix is more than a zoom/translate transform"; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
564 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
565 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
566 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
567 const double sx = t(0,0); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
568 const double sy = t(1,1); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
569 const double sz = t(2,2); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
570 const double w = t(3,3); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
571 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
572 if (LinearAlgebra::IsCloseToZero(sx) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
573 LinearAlgebra::IsCloseToZero(sy) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
574 LinearAlgebra::IsCloseToZero(sz) || |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
575 LinearAlgebra::IsCloseToZero(w)) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
576 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
577 LOG(ERROR) << "Singular transform"; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
578 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
579 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
580 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
581 const double tx = t(0,3); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
582 const double ty = t(1,3); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
583 const double tz = t(2,3); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
584 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
585 Matrix m = IdentityMatrix(4); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
586 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
587 m(0,0) = 1.0 / sx; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
588 m(1,1) = 1.0 / sy; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
589 m(2,2) = 1.0 / sz; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
590 m(3,3) = 1.0 / w; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
591 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
592 m(0,3) = -tx / (sx * w); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
593 m(1,3) = -ty / (sy * w); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
594 m(2,3) = -tz / (sz * w); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
595 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
596 return m; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
597 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
598 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
599 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
600 bool IsShearMatrix(const Matrix& shear) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
601 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
602 return (shear.size1() == 4 && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
603 shear.size2() == 4 && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
604 LinearAlgebra::IsNear(1.0, shear(0,0)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
605 LinearAlgebra::IsNear(0.0, shear(0,1)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
606 LinearAlgebra::IsNear(0.0, shear(0,3)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
607 LinearAlgebra::IsNear(0.0, shear(1,0)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
608 LinearAlgebra::IsNear(1.0, shear(1,1)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
609 LinearAlgebra::IsNear(0.0, shear(1,3)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
610 LinearAlgebra::IsNear(0.0, shear(2,0)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
611 LinearAlgebra::IsNear(0.0, shear(2,1)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
612 LinearAlgebra::IsNear(1.0, shear(2,2)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
613 LinearAlgebra::IsNear(0.0, shear(2,3)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
614 LinearAlgebra::IsNear(0.0, shear(3,0)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
615 LinearAlgebra::IsNear(0.0, shear(3,1)) && |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
616 LinearAlgebra::IsNear(1.0, shear(3,3))); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
617 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
618 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
619 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
620 Matrix InvertShearMatrix(const Matrix& shear) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
621 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
622 if (!IsShearMatrix(shear)) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
623 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
624 LOG(ERROR) << "Not a valid shear matrix"; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
625 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
626 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
627 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
628 Matrix m = IdentityMatrix(4); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
629 m(0,2) = -shear(0,2); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
630 m(1,2) = -shear(1,2); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
631 m(3,2) = -shear(3,2); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
632 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
633 return m; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
164
diff
changeset
|
634 } |
158 | 635 } |
636 } |