Mercurial > hg > orthanc-stone

annotate Framework/Toolbox/LinearAlgebra.cpp @ 212:5412adf19980

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
resort to OrthancFramework
author Sebastien Jodogne <s.jodogne@gmail.com>
date Fri, 01 Jun 2018 17:48:47 +0200
parents e9c7a78a3e77
children d87fe075d31b
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
1 /**
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
2 * Stone of Orthanc
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
4 * Department, University Hospital of Liege, Belgium
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
5 * Copyright (C) 2017-2018 Osimis S.A., Belgium
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
6 *
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
7 * This program is free software: you can redistribute it and/or
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
8 * modify it under the terms of the GNU Affero General Public License
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
9 * as published by the Free Software Foundation, either version 3 of
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
10 * the License, or (at your option) any later version.
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
11 *
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
12 * This program is distributed in the hope that it will be useful, but
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
15 * Affero General Public License for more details.
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
16 *
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
17 * You should have received a copy of the GNU Affero General Public License
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
18 * along with this program. If not, see <http://www.gnu.org/licenses/>.
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
19 **/
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
20
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
21
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
22 #include "LinearAlgebra.h"
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
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
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
27
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
28 #include <stdio.h>
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
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
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
31
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
32 namespace OrthancStone
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
33 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34 namespace LinearAlgebra
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 void Print(const Vector& v)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 for (size_t i = 0; i < v.size(); i++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
39 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
40 printf("%g\n", v[i]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
41 //printf("%8.2f\n", v[i]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
42 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
43 printf("\n");
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
44 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
45
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
46
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
47 void Print(const Matrix& m)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
48 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
49 for (size_t i = 0; i < m.size1(); i++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
50 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
51 for (size_t j = 0; j < m.size2(); j++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
52 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
53 printf("%g ", m(i,j));
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
54 //printf("%8.2f ", m(i,j));
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
56 printf("\n");
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
57 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
58 printf("\n");
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
59 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
60
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
62 bool ParseVector(Vector& target,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63 const std::string& value)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65 std::vector<std::string> items;
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66 Orthanc::Toolbox::TokenizeString(items, value, '\\');
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
68 target.resize(items.size());
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
69
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70 for (size_t i = 0; i < items.size(); i++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
71 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
72 try
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
73 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i]));
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76 catch (boost::bad_lexical_cast&)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
77 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78 target.clear();
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
79 return false;
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
80 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
81 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
82
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
83 return true;
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
84 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
85
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
86
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
87 bool ParseVector(Vector& target,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
88 const Orthanc::DicomMap& dataset,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
89 const Orthanc::DicomTag& tag)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
90 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
91 std::string value;
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
92 return (dataset.CopyToString(value, tag, false) &&
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
93 ParseVector(target, value));
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
94 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
95
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
97 void NormalizeVector(Vector& u)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
98 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
99 double norm = boost::numeric::ublas::norm_2(u);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
100 if (!IsCloseToZero(norm))
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
101 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
102 u = u / norm;
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
103 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
104 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
105
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
106
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
107 void CrossProduct(Vector& result,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
108 const Vector& u,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
109 const Vector& v)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
110 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111 if (u.size() != 3 ||
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112 v.size() != 3)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
113 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
114 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
115 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
116
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
117 result.resize(3);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
118
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
119 result[0] = u[1] * v[2] - u[2] * v[1];
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
120 result[1] = u[2] * v[0] - u[0] * v[2];
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
121 result[2] = u[0] * v[1] - u[1] * v[0];
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
122 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
123
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
124
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
125 void FillMatrix(Matrix& target,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
126 size_t rows,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
127 size_t columns,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128 const double values[])
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
129 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
130 target.resize(rows, columns);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
131
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
132 size_t index = 0;
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
133
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
134 for (size_t y = 0; y < rows; y++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
135 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
136 for (size_t x = 0; x < columns; x++, index++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
137 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
138 target(y, x) = values[index];
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
139 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
140 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
141 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
142
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
143
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 void FillVector(Vector& target,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
145 size_t size,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
146 const double values[])
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
147 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
148 target.resize(size);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
149
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
150 for (size_t i = 0; i < size; i++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
151 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
152 target[i] = values[i];
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
153 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
154 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
155
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
156
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
157 void Convert(Matrix& target,
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
158 const Vector& source)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
159 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
160 const size_t n = source.size();
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
161
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
162 target.resize(n, 1);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
163
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
164 for (size_t i = 0; i < n; i++)
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
165 {
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
166 target(i, 0) = source[i];
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
167 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
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
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
209 using namespace boost::numeric::ublas;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
210
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
211 const Matrix check = prod(trans(q), q) - identity_matrix<double>(3);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
212
160
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 159
diff changeset
213 type_traits<double>::real_type norm = norm_inf(check);
159
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 return (norm <= threshold);
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
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 bool IsOrthogonalMatrix(const Matrix& q)
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 IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon());
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 IsRotationMatrix(const Matrix& r,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
226 double threshold)
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 return (IsOrthogonalMatrix(r, threshold) &&
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
229 IsNear(ComputeDeterminant(r), 1.0, threshold));
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
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
232
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
233 bool IsRotationMatrix(const Matrix& r)
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
234 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
235 return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon());
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 void InvertUpperTriangularMatrix(Matrix& output,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
240 const Matrix& k)
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
241 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
242 if (k.size1() != k.size2())
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 LOG(ERROR) << "Determinant only exists for square matrices";
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
245 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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 output.resize(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 for (size_t i = 1; i < k.size1(); i++)
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
251 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
252 for (size_t j = 0; j < i; j++)
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 if (!IsCloseToZero(k(i, j)))
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 LOG(ERROR) << "Not an upper triangular matrix";
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
257 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
260 output(i, j) = 0; // The output is also upper triangular
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
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
264 if (k.size1() == 3)
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 // https://math.stackexchange.com/a/1004181
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
267 double a = k(0, 0);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
268 double b = k(0, 1);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
269 double c = k(0, 2);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
270 double d = k(1, 1);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
271 double e = k(1, 2);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
272 double f = k(2, 2);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
273
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
274 if (IsCloseToZero(a) ||
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
275 IsCloseToZero(d) ||
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
276 IsCloseToZero(f))
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
277 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
278 LOG(ERROR) << "Singular upper triangular matrix";
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
279 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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 else
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 output(0, 0) = 1.0 / a;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
284 output(0, 1) = -b / (a * d);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
285 output(0, 2) = (b * e - c * d) / (a * f * d);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
286 output(1, 1) = 1.0 / d;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
287 output(1, 2) = -e / (f * d);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
288 output(2, 2) = 1.0 / f;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
289 }
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 else
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
292 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
293 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
294 }
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
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
298 static void GetGivensComponent(double& c,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
299 double& s,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
300 const Matrix& a,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
301 size_t i,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
302 size_t j)
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 assert(i < 3 && j < 3);
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 double x = a(i, i);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
307 double y = a(i, j);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
308 double n = sqrt(x * x + y * y);
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 if (IsCloseToZero(n))
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 c = 1;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
313 s = 0;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
314 }
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
315 else
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
316 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
317 c = x / n;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
318 s = -y / n;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
319 }
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
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 /**
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
324 * This function computes the RQ decomposition of a 3x3 matrix,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
325 * using Givens rotations. Reference: Algorithm A4.1 (page 579) of
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
326 * "Multiple View Geometry in Computer Vision" (2nd edition). The
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
327 * output matrix "Q" is a rotation matrix, and "R" is upper
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
328 * triangular.
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 void RQDecomposition3x3(Matrix& r,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
331 Matrix& q,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
332 const Matrix& a)
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 using namespace boost::numeric::ublas;
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 if (a.size1() != 3 ||
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
337 a.size2() != 3)
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 LOG(ERROR) << "Only applicable to a 3x3 matrix";
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
340 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
343 r.resize(3, 3);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
344 q.resize(3, 3);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
345
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
346 r = a;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
347 q = identity_matrix<double>(3);
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 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
350 // Set A(2,1) to zero
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
351 double c, s;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
352 GetGivensComponent(c, s, r, 2, 1);
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 double v[9] = { 1, 0, 0,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
355 0, c, -s,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
356 0, s, c };
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
357
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
358 Matrix g;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
359 FillMatrix(g, 3, 3, v);
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 r = prod(r, g);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
362 q = prod(trans(g), q);
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 // Set A(2,0) to zero
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
368 double c, s;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
369 GetGivensComponent(c, s, r, 2, 0);
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 double v[9] = { c, 0, -s,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
372 0, 1, 0,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
373 s, 0, c };
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
374
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
375 Matrix g;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
376 FillMatrix(g, 3, 3, v);
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 r = prod(r, g);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
379 q = prod(trans(g), q);
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
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 {
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
384 // Set A(1,0) to zero
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
385 double c, s;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
386 GetGivensComponent(c, s, r, 1, 0);
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 double v[9] = { c, -s, 0,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
389 s, c, 0,
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
390 0, 0, 1 };
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
391
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
392 Matrix g;
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
393 FillMatrix(g, 3, 3, v);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
394
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
395 r = prod(r, g);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
396 q = prod(trans(g), q);
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
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
399 if (!IsCloseToZero(norm_inf(prod(r, q) - a)) ||
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
400 !IsRotationMatrix(q) ||
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
401 !IsCloseToZero(r(1, 0)) ||
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
402 !IsCloseToZero(r(2, 0)) ||
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
403 !IsCloseToZero(r(2, 1)))
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 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
406 }
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
407 }
163
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
408
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
409
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
410 bool InvertMatrixUnsafe(Matrix& target,
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
411 const Matrix& source)
163
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
412 {
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
413 if (source.size1() != source.size2())
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
414 {
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
415 LOG(ERROR) << "Inverse only exists for square matrices";
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
416 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
417 }
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
418
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
419 if (source.size1() < 4)
163
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
420 {
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
421 // For matrices with size below 4, use direct computations
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
422 // instead of LU decomposition
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
423
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
424 if (source.size1() == 0)
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
425 {
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
426 // 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
427 target.resize(0, 0);
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
428 return true;
164
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
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
431 double determinant = ComputeDeterminant(source);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
432
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
433 if (IsCloseToZero(determinant))
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
434 {
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
435 return false;
164
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
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
438 double denominator = 1.0 / determinant;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
439
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
440 target.resize(source.size1(), source.size2());
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
441
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
442 if (source.size1() == 1)
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 target(0, 0) = denominator;
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
445
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
446 return true;
164
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 else if (source.size1() == 2)
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 // 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
451 target(0, 0) = source(1, 1) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
452 target(0, 1) = -source(0, 1) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
453 target(1, 0) = -source(1, 0) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
454 target(1, 1) = source(0, 0) * denominator;
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
455
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
456 return true;
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
457 }
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
458 else if (source.size1() == 3)
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
459 {
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
460 // 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
461 const double a = source(0, 0);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
462 const double b = source(0, 1);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
463 const double c = source(0, 2);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
464 const double d = source(1, 0);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
465 const double e = source(1, 1);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
466 const double f = source(1, 2);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
467 const double g = source(2, 0);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
468 const double h = source(2, 1);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
469 const double i = source(2, 2);
163
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
470
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
471 target(0, 0) = (e * i - f * h) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
472 target(0, 1) = -(b * i - c * h) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
473 target(0, 2) = (b * f - c * e) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
474 target(1, 0) = -(d * i - f * g) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
475 target(1, 1) = (a * i - c * g) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
476 target(1, 2) = -(a * f - c * d) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
477 target(2, 0) = (d * h - e * g) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
478 target(2, 1) = -(a * h - b * g) * denominator;
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
479 target(2, 2) = (a * e - b * d) * denominator;
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
480
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
481 return true;
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
482 }
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
483 else
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
484 {
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
485 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
486 }
163
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
487 }
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
488 else
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
489 {
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
490 // General case, using LU decomposition
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
491
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
492 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
493
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
494 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
495
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
496 if (boost::numeric::ublas::lu_factorize(a, permutation) != 0)
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
497 {
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
498 return false;
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
499 }
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
500 else
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
501 {
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
502 target = boost::numeric::ublas::identity_matrix<double>(source.size1());
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
503 lu_substitute(a, permutation, target);
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
504 return true;
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
505 }
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
506 }
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
507 }
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
508
164
432b1f812d14 inversion of general matrices
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 163
diff changeset
509
169
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
510
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
511 void InvertMatrix(Matrix& target,
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
512 const Matrix& source)
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
513 {
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
514 if (!InvertMatrixUnsafe(target, source))
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
515 {
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
516 LOG(ERROR) << "Cannot invert singular matrix";
7105e51e4907 InvertMatrixUnsafe
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 166
diff changeset
517 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
163
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
518 }
8c5b24892ed2 LinearAlgebra::InvertMatrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
519 }
165
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
520
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
521
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
522 void CreateSkewSymmetric(Matrix& s,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
523 const Vector& v)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
524 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
525 assert(v.size() == 3);
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 s.resize(3, 3);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
528 s(0,0) = 0;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
529 s(0,1) = -v[2];
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
530 s(0,2) = v[1];
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
531 s(1,0) = v[2];
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
532 s(1,1) = 0;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
533 s(1,2) = -v[0];
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
534 s(2,0) = -v[1];
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
535 s(2,1) = v[0];
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
536 s(2,2) = 0;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
537 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
538
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
539
166
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
540 Matrix InvertScalingTranslationMatrix(const Matrix& t)
165
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
541 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
542 if (t.size1() != 4 ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
543 t.size2() != 4 ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
544 !LinearAlgebra::IsCloseToZero(t(0,1)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
545 !LinearAlgebra::IsCloseToZero(t(0,2)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
546 !LinearAlgebra::IsCloseToZero(t(1,0)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
547 !LinearAlgebra::IsCloseToZero(t(1,2)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
548 !LinearAlgebra::IsCloseToZero(t(2,0)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
549 !LinearAlgebra::IsCloseToZero(t(2,1)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
550 !LinearAlgebra::IsCloseToZero(t(3,0)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
551 !LinearAlgebra::IsCloseToZero(t(3,1)) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
552 !LinearAlgebra::IsCloseToZero(t(3,2)))
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
553 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
554 LOG(ERROR) << "This matrix is more than a zoom/translate transform";
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
555 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
556 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
557
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
558 const double sx = t(0,0);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
559 const double sy = t(1,1);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
560 const double sz = t(2,2);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
561 const double w = t(3,3);
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 if (LinearAlgebra::IsCloseToZero(sx) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
564 LinearAlgebra::IsCloseToZero(sy) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
565 LinearAlgebra::IsCloseToZero(sz) ||
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
566 LinearAlgebra::IsCloseToZero(w))
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
567 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
568 LOG(ERROR) << "Singular transform";
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
569 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
570 }
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 const double tx = t(0,3);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
573 const double ty = t(1,3);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
574 const double tz = t(2,3);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
575
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
576 Matrix m = IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
577
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
578 m(0,0) = 1.0 / sx;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
579 m(1,1) = 1.0 / sy;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
580 m(2,2) = 1.0 / sz;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
581 m(3,3) = 1.0 / w;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
582
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
583 m(0,3) = -tx / (sx * w);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
584 m(1,3) = -ty / (sy * w);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
585 m(2,3) = -tz / (sz * w);
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 return m;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
588 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
589
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
590
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
591 bool IsShearMatrix(const Matrix& shear)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
592 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
593 return (shear.size1() == 4 &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
594 shear.size2() == 4 &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
595 LinearAlgebra::IsNear(1.0, shear(0,0)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
596 LinearAlgebra::IsNear(0.0, shear(0,1)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
597 LinearAlgebra::IsNear(0.0, shear(0,3)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
598 LinearAlgebra::IsNear(0.0, shear(1,0)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
599 LinearAlgebra::IsNear(1.0, shear(1,1)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
600 LinearAlgebra::IsNear(0.0, shear(1,3)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
601 LinearAlgebra::IsNear(0.0, shear(2,0)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
602 LinearAlgebra::IsNear(0.0, shear(2,1)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
603 LinearAlgebra::IsNear(1.0, shear(2,2)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
604 LinearAlgebra::IsNear(0.0, shear(2,3)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
605 LinearAlgebra::IsNear(0.0, shear(3,0)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
606 LinearAlgebra::IsNear(0.0, shear(3,1)) &&
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
607 LinearAlgebra::IsNear(1.0, shear(3,3)));
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
608 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
609
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
610
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
611 Matrix InvertShearMatrix(const Matrix& shear)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
612 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
613 if (!IsShearMatrix(shear))
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
614 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
615 LOG(ERROR) << "Not a valid shear matrix";
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
616 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
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 Matrix m = IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
620 m(0,2) = -shear(0,2);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
621 m(1,2) = -shear(1,2);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
622 m(3,2) = -shear(3,2);
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 return m;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 164
diff changeset
625 }
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
626 }
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
627 }