Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/LinearAlgebra.cpp @ 159:0a73d76333db wasm
populating LinearAlgebra
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Wed, 14 Feb 2018 09:15:08 +0100 |
parents | a053ca7fa5c6 |
children | e9dae7e7bffc |
comparison
equal
deleted
inserted
replaced
158:a053ca7fa5c6 | 159:0a73d76333db |
---|---|
194 for (size_t i = 0; i < n; i++) | 194 for (size_t i = 0; i < n; i++) |
195 { | 195 { |
196 target(i, 0) = source[i]; | 196 target(i, 0) = source[i]; |
197 } | 197 } |
198 } | 198 } |
199 | |
200 | |
201 double ComputeDeterminant(const Matrix& a) | |
202 { | |
203 if (a.size1() != a.size2()) | |
204 { | |
205 LOG(ERROR) << "Determinant only exists for square matrices"; | |
206 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
207 } | |
208 | |
209 // https://en.wikipedia.org/wiki/Rule_of_Sarrus | |
210 if (a.size1() == 1) | |
211 { | |
212 return a(0,0); | |
213 } | |
214 else if (a.size1() == 2) | |
215 { | |
216 return a(0,0) * a(1,1) - a(0,1) * a(1,0); | |
217 } | |
218 else if (a.size1() == 3) | |
219 { | |
220 return (a(0,0) * a(1,1) * a(2,2) + | |
221 a(0,1) * a(1,2) * a(2,0) + | |
222 a(0,2) * a(1,0) * a(2,1) - | |
223 a(2,0) * a(1,1) * a(0,2) - | |
224 a(2,1) * a(1,2) * a(0,0) - | |
225 a(2,2) * a(1,0) * a(0,1)); | |
226 } | |
227 else | |
228 { | |
229 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
230 } | |
231 } | |
232 | |
233 | |
234 bool IsOrthogonalMatrix(const Matrix& q, | |
235 double threshold) | |
236 { | |
237 // https://en.wikipedia.org/wiki/Orthogonal_matrix | |
238 | |
239 using namespace boost::numeric::ublas; | |
240 | |
241 const Matrix check = prod(trans(q), q) - identity_matrix<double>(3); | |
242 | |
243 typename type_traits<double>::real_type norm = norm_inf(check); | |
244 | |
245 return (norm <= threshold); | |
246 } | |
247 | |
248 | |
249 bool IsOrthogonalMatrix(const Matrix& q) | |
250 { | |
251 return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon()); | |
252 } | |
253 | |
254 | |
255 bool IsRotationMatrix(const Matrix& r, | |
256 double threshold) | |
257 { | |
258 return (IsOrthogonalMatrix(r, threshold) && | |
259 IsNear(ComputeDeterminant(r), 1.0, threshold)); | |
260 } | |
261 | |
262 | |
263 bool IsRotationMatrix(const Matrix& r) | |
264 { | |
265 return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon()); | |
266 } | |
267 | |
268 | |
269 void InvertUpperTriangularMatrix(Matrix& output, | |
270 const Matrix& k) | |
271 { | |
272 if (k.size1() != k.size2()) | |
273 { | |
274 LOG(ERROR) << "Determinant only exists for square matrices"; | |
275 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
276 } | |
277 | |
278 output.resize(k.size1(), k.size2()); | |
279 | |
280 for (size_t i = 1; i < k.size1(); i++) | |
281 { | |
282 for (size_t j = 0; j < i; j++) | |
283 { | |
284 if (!IsCloseToZero(k(i, j))) | |
285 { | |
286 LOG(ERROR) << "Not an upper triangular matrix"; | |
287 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
288 } | |
289 | |
290 output(i, j) = 0; // The output is also upper triangular | |
291 } | |
292 } | |
293 | |
294 if (k.size1() == 3) | |
295 { | |
296 // https://math.stackexchange.com/a/1004181 | |
297 double a = k(0, 0); | |
298 double b = k(0, 1); | |
299 double c = k(0, 2); | |
300 double d = k(1, 1); | |
301 double e = k(1, 2); | |
302 double f = k(2, 2); | |
303 | |
304 if (IsCloseToZero(a) || | |
305 IsCloseToZero(d) || | |
306 IsCloseToZero(f)) | |
307 { | |
308 LOG(ERROR) << "Singular upper triangular matrix"; | |
309 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
310 } | |
311 else | |
312 { | |
313 output(0, 0) = 1.0 / a; | |
314 output(0, 1) = -b / (a * d); | |
315 output(0, 2) = (b * e - c * d) / (a * f * d); | |
316 output(1, 1) = 1.0 / d; | |
317 output(1, 2) = -e / (f * d); | |
318 output(2, 2) = 1.0 / f; | |
319 } | |
320 } | |
321 else | |
322 { | |
323 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
324 } | |
325 } | |
326 | |
327 | |
328 static void GetGivensComponent(double& c, | |
329 double& s, | |
330 const Matrix& a, | |
331 size_t i, | |
332 size_t j) | |
333 { | |
334 assert(i < 3 && j < 3); | |
335 | |
336 double x = a(i, i); | |
337 double y = a(i, j); | |
338 double n = sqrt(x * x + y * y); | |
339 | |
340 if (IsCloseToZero(n)) | |
341 { | |
342 c = 1; | |
343 s = 0; | |
344 } | |
345 else | |
346 { | |
347 c = x / n; | |
348 s = -y / n; | |
349 } | |
350 } | |
351 | |
352 | |
353 /** | |
354 * This function computes the RQ decomposition of a 3x3 matrix, | |
355 * using Givens rotations. Reference: Algorithm A4.1 (page 579) of | |
356 * "Multiple View Geometry in Computer Vision" (2nd edition). The | |
357 * output matrix "Q" is a rotation matrix, and "R" is upper | |
358 * triangular. | |
359 **/ | |
360 void RQDecomposition3x3(Matrix& r, | |
361 Matrix& q, | |
362 const Matrix& a) | |
363 { | |
364 using namespace boost::numeric::ublas; | |
365 | |
366 if (a.size1() != 3 || | |
367 a.size2() != 3) | |
368 { | |
369 LOG(ERROR) << "Only applicable to a 3x3 matrix"; | |
370 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
371 } | |
372 | |
373 r.resize(3, 3); | |
374 q.resize(3, 3); | |
375 | |
376 r = a; | |
377 q = identity_matrix<double>(3); | |
378 | |
379 { | |
380 // Set A(2,1) to zero | |
381 double c, s; | |
382 GetGivensComponent(c, s, r, 2, 1); | |
383 | |
384 double v[9] = { 1, 0, 0, | |
385 0, c, -s, | |
386 0, s, c }; | |
387 | |
388 Matrix g; | |
389 FillMatrix(g, 3, 3, v); | |
390 | |
391 r = prod(r, g); | |
392 q = prod(trans(g), q); | |
393 } | |
394 | |
395 | |
396 { | |
397 // Set A(2,0) to zero | |
398 double c, s; | |
399 GetGivensComponent(c, s, r, 2, 0); | |
400 | |
401 double v[9] = { c, 0, -s, | |
402 0, 1, 0, | |
403 s, 0, c }; | |
404 | |
405 Matrix g; | |
406 FillMatrix(g, 3, 3, v); | |
407 | |
408 r = prod(r, g); | |
409 q = prod(trans(g), q); | |
410 } | |
411 | |
412 | |
413 { | |
414 // Set A(1,0) to zero | |
415 double c, s; | |
416 GetGivensComponent(c, s, r, 1, 0); | |
417 | |
418 double v[9] = { c, -s, 0, | |
419 s, c, 0, | |
420 0, 0, 1 }; | |
421 | |
422 Matrix g; | |
423 FillMatrix(g, 3, 3, v); | |
424 | |
425 r = prod(r, g); | |
426 q = prod(trans(g), q); | |
427 } | |
428 | |
429 if (!IsCloseToZero(norm_inf(prod(r, q) - a)) || | |
430 !IsRotationMatrix(q) || | |
431 !IsCloseToZero(r(1, 0)) || | |
432 !IsCloseToZero(r(2, 0)) || | |
433 !IsCloseToZero(r(2, 1))) | |
434 { | |
435 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
436 } | |
437 } | |
199 } | 438 } |
200 } | 439 } |