Mercurial > hg > orthanc-stone
annotate Framework/Toolbox/LinearAlgebra.cpp @ 161:197a5ddaf68c wasm
FiniteProjectiveCamera
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Wed, 14 Feb 2018 11:29:26 +0100 |
parents | e9dae7e7bffc |
children | 8c5b24892ed2 |
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 | |
5 * Copyright (C) 2017-2018 Osimis S.A., Belgium | |
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 | |
24 #include <Core/Logging.h> | |
25 #include <Core/OrthancException.h> | |
26 #include <Core/Toolbox.h> | |
27 | |
28 #include <stdio.h> | |
29 #include <boost/lexical_cast.hpp> | |
30 | |
31 namespace OrthancStone | |
32 { | |
33 namespace LinearAlgebra | |
34 { | |
35 void Print(const Vector& v) | |
36 { | |
37 for (size_t i = 0; i < v.size(); i++) | |
38 { | |
39 printf("%g\n", v[i]); | |
40 //printf("%8.2f\n", v[i]); | |
41 } | |
42 printf("\n"); | |
43 } | |
44 | |
45 | |
46 void Print(const Matrix& m) | |
47 { | |
48 for (size_t i = 0; i < m.size1(); i++) | |
49 { | |
50 for (size_t j = 0; j < m.size2(); j++) | |
51 { | |
52 printf("%g ", m(i,j)); | |
53 //printf("%8.2f ", m(i,j)); | |
54 } | |
55 printf("\n"); | |
56 } | |
57 printf("\n"); | |
58 } | |
59 | |
60 | |
61 bool ParseVector(Vector& target, | |
62 const std::string& value) | |
63 { | |
64 std::vector<std::string> items; | |
65 Orthanc::Toolbox::TokenizeString(items, value, '\\'); | |
66 | |
67 target.resize(items.size()); | |
68 | |
69 for (size_t i = 0; i < items.size(); i++) | |
70 { | |
71 try | |
72 { | |
73 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i])); | |
74 } | |
75 catch (boost::bad_lexical_cast&) | |
76 { | |
77 target.clear(); | |
78 return false; | |
79 } | |
80 } | |
81 | |
82 return true; | |
83 } | |
84 | |
85 | |
86 bool ParseVector(Vector& target, | |
87 const Orthanc::DicomMap& dataset, | |
88 const Orthanc::DicomTag& tag) | |
89 { | |
90 std::string value; | |
91 return (dataset.CopyToString(value, tag, false) && | |
92 ParseVector(target, value)); | |
93 } | |
94 | |
95 | |
96 void AssignVector(Vector& v, | |
97 double v1, | |
98 double v2) | |
99 { | |
100 v.resize(2); | |
101 v[0] = v1; | |
102 v[1] = v2; | |
103 } | |
104 | |
105 | |
106 void AssignVector(Vector& v, | |
107 double v1, | |
108 double v2, | |
109 double v3) | |
110 { | |
111 v.resize(3); | |
112 v[0] = v1; | |
113 v[1] = v2; | |
114 v[2] = v3; | |
115 } | |
116 | |
117 | |
161
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
118 void AssignVector(Vector& v, |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
119 double v1, |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
120 double v2, |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
121 double v3, |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
122 double v4) |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
123 { |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
124 v.resize(4); |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
125 v[0] = v1; |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
126 v[1] = v2; |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
127 v[2] = v3; |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
128 v[3] = v4; |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
129 } |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
130 |
197a5ddaf68c
FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
160
diff
changeset
|
131 |
158 | 132 bool IsNear(double x, |
133 double y) | |
134 { | |
135 // As most input is read as single-precision numbers, we take the | |
136 // epsilon machine for float32 into consideration to compare numbers | |
137 return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon()); | |
138 } | |
139 | |
140 | |
141 void NormalizeVector(Vector& u) | |
142 { | |
143 double norm = boost::numeric::ublas::norm_2(u); | |
144 if (!IsCloseToZero(norm)) | |
145 { | |
146 u = u / norm; | |
147 } | |
148 } | |
149 | |
150 | |
151 void CrossProduct(Vector& result, | |
152 const Vector& u, | |
153 const Vector& v) | |
154 { | |
155 if (u.size() != 3 || | |
156 v.size() != 3) | |
157 { | |
158 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
159 } | |
160 | |
161 result.resize(3); | |
162 | |
163 result[0] = u[1] * v[2] - u[2] * v[1]; | |
164 result[1] = u[2] * v[0] - u[0] * v[2]; | |
165 result[2] = u[0] * v[1] - u[1] * v[0]; | |
166 } | |
167 | |
168 | |
169 void FillMatrix(Matrix& target, | |
170 size_t rows, | |
171 size_t columns, | |
172 const double values[]) | |
173 { | |
174 target.resize(rows, columns); | |
175 | |
176 size_t index = 0; | |
177 | |
178 for (size_t y = 0; y < rows; y++) | |
179 { | |
180 for (size_t x = 0; x < columns; x++, index++) | |
181 { | |
182 target(y, x) = values[index]; | |
183 } | |
184 } | |
185 } | |
186 | |
187 | |
188 void FillVector(Vector& target, | |
189 size_t size, | |
190 const double values[]) | |
191 { | |
192 target.resize(size); | |
193 | |
194 for (size_t i = 0; i < size; i++) | |
195 { | |
196 target[i] = values[i]; | |
197 } | |
198 } | |
199 | |
200 | |
201 void Convert(Matrix& target, | |
202 const Vector& source) | |
203 { | |
204 const size_t n = source.size(); | |
205 | |
206 target.resize(n, 1); | |
207 | |
208 for (size_t i = 0; i < n; i++) | |
209 { | |
210 target(i, 0) = source[i]; | |
211 } | |
212 } | |
159
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
213 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
214 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
215 double ComputeDeterminant(const Matrix& a) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
216 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
217 if (a.size1() != a.size2()) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
218 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
219 LOG(ERROR) << "Determinant only exists for square matrices"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
220 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
221 } |
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 // https://en.wikipedia.org/wiki/Rule_of_Sarrus |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
224 if (a.size1() == 1) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
225 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
226 return a(0,0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
227 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
228 else if (a.size1() == 2) |
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 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
|
231 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
232 else if (a.size1() == 3) |
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 (a(0,0) * a(1,1) * a(2,2) + |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
235 a(0,1) * a(1,2) * a(2,0) + |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
236 a(0,2) * a(1,0) * a(2,1) - |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
237 a(2,0) * a(1,1) * a(0,2) - |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
238 a(2,1) * a(1,2) * a(0,0) - |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
239 a(2,2) * a(1,0) * a(0,1)); |
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 else |
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 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); |
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 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
246 |
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 bool IsOrthogonalMatrix(const Matrix& q, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
249 double threshold) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
250 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
251 // https://en.wikipedia.org/wiki/Orthogonal_matrix |
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 using namespace boost::numeric::ublas; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
254 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
255 const Matrix check = prod(trans(q), q) - identity_matrix<double>(3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
256 |
160 | 257 type_traits<double>::real_type norm = norm_inf(check); |
159
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
258 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
259 return (norm <= threshold); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
260 } |
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 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
263 bool IsOrthogonalMatrix(const Matrix& q) |
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 return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon()); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
266 } |
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 bool IsRotationMatrix(const Matrix& r, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
270 double threshold) |
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 return (IsOrthogonalMatrix(r, threshold) && |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
273 IsNear(ComputeDeterminant(r), 1.0, threshold)); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
274 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
275 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
276 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
277 bool IsRotationMatrix(const Matrix& r) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
278 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
279 return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon()); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
280 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
281 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
282 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
283 void InvertUpperTriangularMatrix(Matrix& output, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
284 const Matrix& k) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
285 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
286 if (k.size1() != k.size2()) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
287 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
288 LOG(ERROR) << "Determinant only exists for square matrices"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
289 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
290 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
291 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
292 output.resize(k.size1(), k.size2()); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
293 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
294 for (size_t i = 1; i < k.size1(); i++) |
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 for (size_t j = 0; j < i; j++) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
297 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
298 if (!IsCloseToZero(k(i, j))) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
299 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
300 LOG(ERROR) << "Not an upper triangular matrix"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
301 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
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 output(i, j) = 0; // The output is also upper triangular |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
305 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
306 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
307 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
308 if (k.size1() == 3) |
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 // https://math.stackexchange.com/a/1004181 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
311 double a = k(0, 0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
312 double b = k(0, 1); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
313 double c = k(0, 2); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
314 double d = k(1, 1); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
315 double e = k(1, 2); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
316 double f = k(2, 2); |
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 if (IsCloseToZero(a) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
319 IsCloseToZero(d) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
320 IsCloseToZero(f)) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
321 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
322 LOG(ERROR) << "Singular upper triangular matrix"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
323 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
324 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
325 else |
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 output(0, 0) = 1.0 / a; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
328 output(0, 1) = -b / (a * d); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
329 output(0, 2) = (b * e - c * d) / (a * f * d); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
330 output(1, 1) = 1.0 / d; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
331 output(1, 2) = -e / (f * d); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
332 output(2, 2) = 1.0 / f; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
333 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
334 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
335 else |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
336 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
337 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
338 } |
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 |
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 static void GetGivensComponent(double& c, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
343 double& s, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
344 const Matrix& a, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
345 size_t i, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
346 size_t j) |
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 assert(i < 3 && j < 3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
349 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
350 double x = a(i, i); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
351 double y = a(i, j); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
352 double n = sqrt(x * x + y * y); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
353 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
354 if (IsCloseToZero(n)) |
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 c = 1; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
357 s = 0; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
358 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
359 else |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
360 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
361 c = x / n; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
362 s = -y / n; |
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 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
365 |
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 /** |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
368 * This function computes the RQ decomposition of a 3x3 matrix, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
369 * using Givens rotations. Reference: Algorithm A4.1 (page 579) of |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
370 * "Multiple View Geometry in Computer Vision" (2nd edition). The |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
371 * output matrix "Q" is a rotation matrix, and "R" is upper |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
372 * triangular. |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
373 **/ |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
374 void RQDecomposition3x3(Matrix& r, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
375 Matrix& q, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
376 const Matrix& a) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
377 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
378 using namespace boost::numeric::ublas; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
379 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
380 if (a.size1() != 3 || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
381 a.size2() != 3) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
382 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
383 LOG(ERROR) << "Only applicable to a 3x3 matrix"; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
384 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
385 } |
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 r.resize(3, 3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
388 q.resize(3, 3); |
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 r = a; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
391 q = identity_matrix<double>(3); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
392 |
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 // Set A(2,1) to zero |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
395 double c, s; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
396 GetGivensComponent(c, s, r, 2, 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 double v[9] = { 1, 0, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
399 0, c, -s, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
400 0, s, c }; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
401 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
402 Matrix g; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
403 FillMatrix(g, 3, 3, v); |
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 r = prod(r, g); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
406 q = prod(trans(g), q); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
407 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
408 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
409 |
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 // Set A(2,0) to zero |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
412 double c, s; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
413 GetGivensComponent(c, s, r, 2, 0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
414 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
415 double v[9] = { c, 0, -s, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
416 0, 1, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
417 s, 0, c }; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
418 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
419 Matrix g; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
420 FillMatrix(g, 3, 3, v); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
421 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
422 r = prod(r, g); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
423 q = prod(trans(g), q); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
424 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
425 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
426 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
427 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
428 // Set A(1,0) to zero |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
429 double c, s; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
430 GetGivensComponent(c, s, r, 1, 0); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
431 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
432 double v[9] = { c, -s, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
433 s, c, 0, |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
434 0, 0, 1 }; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
435 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
436 Matrix g; |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
437 FillMatrix(g, 3, 3, v); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
438 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
439 r = prod(r, g); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
440 q = prod(trans(g), q); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
441 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
442 |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
443 if (!IsCloseToZero(norm_inf(prod(r, q) - a)) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
444 !IsRotationMatrix(q) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
445 !IsCloseToZero(r(1, 0)) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
446 !IsCloseToZero(r(2, 0)) || |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
447 !IsCloseToZero(r(2, 1))) |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
448 { |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
449 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
450 } |
0a73d76333db
populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
451 } |
158 | 452 } |
453 } |